diff --git a/.cproject b/.cproject index 1f29f86b7..c53d6aaaf 100644 --- a/.cproject +++ b/.cproject @@ -761,6 +761,14 @@ true true + + make + -j5 + testDiscreteMarginals.run + true + true + true + make -j2 @@ -1653,10 +1661,10 @@ true true - + make - -j5 - PlanarSLAMExample.run + -j2 + PlanarSLAMExample_easy.run true true true @@ -1685,10 +1693,10 @@ true true - + make - -j5 - PlanarSLAMExample_selfcontained.run + -j2 + PlanarSLAMSelfContained_advanced.run true true true @@ -1701,18 +1709,18 @@ true true - + make - -j5 - Pose2SLAMExample.run + -j2 + Pose2SLAMExample_easy.run true true true - + make - -j5 - Pose2SLAMwSPCG.run + -j2 + Pose2SLAMwSPCG_easy.run true true true @@ -1741,10 +1749,10 @@ true true - + make -j5 - Pose2SLAMExample_graph.run + Pose2SLAMwSPCG_advanced.run true true true @@ -2152,6 +2160,38 @@ true true + + make + -j5 + wrap_gtsam_build + true + true + true + + + make + -j5 + wrap_gtsam_unstable_build + true + true + true + + + make + -j5 + wrap_gtsam_unstable + true + true + true + + + make + -j5 + wrap + true + true + true + make -j5 diff --git a/CMakeLists.txt b/CMakeLists.txt index 335fc76c1..3eb4983b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,8 +56,13 @@ option(GTSAM_INSTALL_MATLAB_EXAMPLES "Enable/Disable installation of matlab option(GTSAM_INSTALL_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON) option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON) -# Experimental - features disabled by default -option(GTSAM_ENABLE_BUILD_MEX_BINARIES "Enable/Disable building of matlab mex files" OFF) +# TODO: Check for matlab mex binary before handling building of binaries + +# Flags for building/installing mex files +option(GTSAM_ENABLE_BUILD_MEX_BINARIES "Enable/Disable building of matlab mex files" OFF) +option(GTSAM_ENABLE_BUILD_MEX_BINARIES_ALL "Enable/Disable adding building of mex files to ALL target" OFF) +set(GTSAM_BUILD_MEX_BINARY_FLAGS "-j2" CACHE STRING "Flags for running make on toolbox MEX files") +set(MEX_COMMAND "mex" CACHE STRING "Command to use for executing mex (if on path, 'mex' will work)") # Flags for choosing default packaging tools set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") @@ -187,6 +192,8 @@ print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox print_config_flag(${GTSAM_INSTALL_MATLAB_EXAMPLES} "Install matlab examples ") print_config_flag(${GTSAM_INSTALL_MATLAB_TESTS} "Install matlab tests ") print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ") +print_config_flag(${GTSAM_ENABLE_BUILD_MEX_BINARIES} "Build MEX binaries ") +print_config_flag(${GTSAM_ENABLE_BUILD_MEX_BINARIES_ALL} "Build MEX binaries on ALL target ") message(STATUS "===============================================================") # Include CPack *after* all flags diff --git a/doc/LieGroups.lyx b/doc/LieGroups.lyx new file mode 100644 index 000000000..03116dbc9 --- /dev/null +++ b/doc/LieGroups.lyx @@ -0,0 +1,4065 @@ +#LyX 2.0 created this file. For more info see http://www.lyx.org/ +\lyxformat 413 +\begin_document +\begin_header +\textclass article +\use_default_options false +\begin_modules +theorems-std +\end_modules +\maintain_unincluded_children false +\language english +\language_package default +\inputencoding auto +\fontencoding global +\font_roman times +\font_sans default +\font_typewriter default +\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 +\bibtex_command default +\index_command default +\paperfontsize 12 +\spacing single +\use_hyperref false +\papersize default +\use_geometry true +\use_amsmath 1 +\use_esint 0 +\use_mhchem 1 +\use_mathdots 1 +\cite_engine basic +\use_bibtopic false +\use_indices false +\paperorientation portrait +\suppress_date false +\use_refstyle 0 +\index Index +\shortcut idx +\color #008000 +\end_index +\leftmargin 1in +\topmargin 1in +\rightmargin 1in +\bottommargin 1in +\secnumdepth 3 +\tocdepth 3 +\paragraph_separation indent +\paragraph_indentation default +\quotes_language english +\papercolumns 1 +\papersides 1 +\paperpagestyle default +\tracking_changes false +\output_changes false +\html_math_output 0 +\html_css_as_file 0 +\html_be_strict false +\end_header + +\begin_body + +\begin_layout Title +Lie Groups for Beginners +\end_layout + +\begin_layout Author +Frank Dellaert +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +Derivatives +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}} +{\frac{\partial#1}{\partial#2}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\at}[2]{#1\biggr\rvert_{#2}} +{#1\biggr\rvert_{#2}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} } +{\at{\deriv{#1}{#2}}{#3}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +Lie Groups +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\xhat}{\hat{x}} +{\hat{x}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\yhat}{\hat{y}} +{\hat{y}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Ad}[1]{Ad_{#1}} +{Ad_{#1}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}} +{\mathbf{\mathop{Ad}}{}_{#1}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\define}{\stackrel{\Delta}{=}} +{\stackrel{\Delta}{=}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\gg}{\mathfrak{g}} +{\mathfrak{g}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Rn}{\mathbb{R}^{n}} +{\mathbb{R}^{n}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SO(2), 1 +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}} +{\mathfrak{\mathbb{R}^{2}}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\SOtwo}{SO(2)} +{SO(2)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\sotwo}{\mathfrak{so(2)}} +{\mathfrak{so(2)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\that}{\hat{\theta}} +{\hat{\theta}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\skew}[1]{[#1]_{+}} +{[#1]_{+}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SE(2), 3 +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\SEtwo}{SE(2)} +{SE(2)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\setwo}{\mathfrak{se(2)}} +{\mathfrak{se(2)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Skew}[1]{[#1]_{\times}} +{[#1]_{\times}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SO(3), 3 +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rthree}{\mathfrak{\mathbb{R}^{3}}} +{\mathfrak{\mathbb{R}^{3}}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\SOthree}{SO(3)} +{SO(3)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\sothree}{\mathfrak{so(3)}} +{\mathfrak{so(3)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\what}{\hat{\omega}} +{\hat{\omega}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SE(3),6 +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rsix}{\mathfrak{\mathbb{R}^{6}}} +{\mathfrak{\mathbb{R}^{6}}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\SEthree}{SE(3)} +{SE(3)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\sethree}{\mathfrak{se(3)}} +{\mathfrak{se(3)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\xihat}{\hat{\xi}} +{\hat{\xi}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +Aff(2),6 +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Afftwo}{Aff(2)} +{Aff(2)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\afftwo}{\mathfrak{aff(2)}} +{\mathfrak{aff(2)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\aa}{a} +{a} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\ahat}{\hat{a}} +{\hat{a}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status collapsed + +\begin_layout Plain Layout +SL(3),8 +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\SLthree}{SL(3)} +{SL(3)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\slthree}{\mathfrak{sl(3)}} +{\mathfrak{sl(3)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\hh}{h} +{h} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\hhat}{\hat{h}} +{\hat{h}} +\end_inset + + +\end_layout + +\begin_layout Section +Motivation: Rigid Motions in the Plane +\end_layout + +\begin_layout Standard +We will start with a small example of a robot moving in a plane, parameterized + by a +\emph on +2D pose +\emph default + +\begin_inset Formula $(x,\, y,\,\theta)$ +\end_inset + +. + When we give it a small forward velocity +\begin_inset Formula $v_{x}$ +\end_inset + +, we know that the location changes as +\begin_inset Formula +\[ +\dot{x}=v_{x} +\] + +\end_inset + +The solution to this trivial differential equation is, with +\begin_inset Formula $x_{0}$ +\end_inset + + the initial +\begin_inset Formula $x$ +\end_inset + +-position of the robot, +\begin_inset Formula +\[ +x_{t}=x_{0}+v_{x}t +\] + +\end_inset + +A similar story holds for translation in the +\begin_inset Formula $y$ +\end_inset + + 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}) +\] + +\end_inset + +Similarly for rotation we have +\begin_inset Formula +\[ +(x_{t},\, y_{t},\,\theta_{t})=(x_{0},\, y_{0},\,\theta_{0}+\omega t) +\] + +\end_inset + +where +\begin_inset Formula $\omega$ +\end_inset + + is angular velocity, measured in +\begin_inset Formula $rad/s$ +\end_inset + + in counterclockwise direction. + +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status collapsed + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/circular.pdf + +\end_inset + + +\begin_inset Caption + +\begin_layout Plain Layout +Robot moving along a circular trajectory. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +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) +\] + +\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)$ +\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) +\] + +\end_inset + +but now the robot has rotated, and for the next incremental change, the + velocity vector would have to be rotated before it can be applied. + In fact, the robot will move on a +\emph on +circular +\emph default + trajectory. + +\end_layout + +\begin_layout Standard +The reason is that +\emph on +translation and rotation do not commute +\emph default +: if we rotate and then move we will end up in a different place than if + we moved first, then rotated. + In fact, someone once said (I forget who, kudos for who can track down + the exact quote): +\end_layout + +\begin_layout Quote +If rotation and translation commuted, we could do all rotations before leaving + home. +\end_layout + +\begin_layout Standard +\begin_inset Float figure +placement h +wide false +sideways false +status open + +\begin_layout Plain Layout +\align center +\begin_inset Graphics + filename images/n-steps.pdf + +\end_inset + + +\begin_inset Caption + +\begin_layout Plain Layout +\begin_inset CommandInset label +LatexCommand label +name "fig:n-step-program" + +\end_inset + +Approximating a circular trajectory with +\begin_inset Formula $n$ +\end_inset + + steps. +\end_layout + +\end_inset + + +\end_layout + +\end_inset + +To make progress, we have to be more precise about how the robot behaves. + Specifically, let us define composition of two poses +\begin_inset Formula $T_{1}$ +\end_inset + + and +\begin_inset Formula $T_{2}$ +\end_inset + + 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}) +\] + +\end_inset + +This is a bit clumsy, so we resort to a trick: embed the 2D poses in the + space of +\begin_inset Formula $3\times3$ +\end_inset + + matrices, so we can define composition as matrix multiplication: +\begin_inset Formula +\[ +T_{1}T_{2}=\left[\begin{array}{cc} +R_{1} & t_{1}\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +R_{2} & t_{2}\\ +0 & 1 +\end{array}\right]=\left[\begin{array}{cc} +R_{1}R_{2} & R_{1}t_{2}+t_{1}\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + +where the matrices +\begin_inset Formula $R$ +\end_inset + + are 2D rotation matrices defined as +\begin_inset Formula +\[ +R=\left[\begin{array}{cc} +\cos\theta & -\sin\theta\\ +\sin\theta & \cos\theta +\end{array}\right] +\] + +\end_inset + +Now a +\begin_inset Quotes eld +\end_inset + +tiny +\begin_inset Quotes erd +\end_inset + + motion of the robot can be written as +\begin_inset Formula +\[ +T(\delta)=\left[\begin{array}{ccc} +\cos\omega\delta & -\sin\omega\delta & v_{x}\delta\\ +\sin\omega\delta & \cos\omega\delta & v_{y}\delta\\ +0 & 0 & 1 +\end{array}\right]\approx\left[\begin{array}{ccc} +1 & -\omega\delta & v_{x}\delta\\ +\omega\delta & 1 & v_{y}\delta\\ +0 & 0 & 1 +\end{array}\right]=I+\delta\left[\begin{array}{ccc} +0 & -\omega & v_{x}\\ +\omega & 0 & v_{y}\\ +0 & 0 & 0 +\end{array}\right] +\] + +\end_inset + +Let us define the +\emph on +2D twist +\emph default + vector +\begin_inset Formula $\xi=(v,\omega)$ +\end_inset + +, and the matrix above as +\begin_inset Formula +\[ +\xihat\define\left[\begin{array}{ccc} +0 & -\omega & v_{x}\\ +\omega & 0 & v_{y}\\ +0 & 0 & 0 +\end{array}\right] +\] + +\end_inset + +If we wanted +\begin_inset Formula $t$ +\end_inset + + to be large, we could split up +\begin_inset Formula $t$ +\end_inset + + into smaller timesteps, say +\begin_inset Formula $n$ +\end_inset + + of them, and compose them as follows: +\begin_inset Formula +\[ +T(t)\approx\left(I+\frac{t}{n}\xihat\right)\ldots\mbox{n times}\ldots\left(I+\frac{t}{n}\xihat\right)=\left(I+\frac{t}{n}\xihat\right)^{n} +\] + +\end_inset + +The result is shown in Figure +\begin_inset CommandInset ref +LatexCommand ref +reference "fig:n-step-program" + +\end_inset + +. +\end_layout + +\begin_layout Standard +Of course, the perfect solution would be obtained if we take +\begin_inset Formula $n$ +\end_inset + + to infinity: +\begin_inset Formula +\[ +T(t)=\lim_{n\rightarrow\infty}\left(I+\frac{t}{n}\xihat\right)^{n} +\] + +\end_inset + +For real numbers, this series is familiar and is actually a way to compute + the exponential function: +\begin_inset Formula +\[ +e^{x}=\lim_{n\rightarrow\infty}\left(1+\frac{x}{n}\right)^{n}=\sum_{k=0}^{\infty}\frac{x^{k}}{k!} +\] + +\end_inset + +The series can be similarly defined for square matrices, and the final result + is that we can write the motion of a robot along a circular trajectory, + resulting from the 2D twist +\begin_inset Formula $\xi=(v,\omega)$ +\end_inset + + +\begin_inset Formula $ $ +\end_inset + + as the +\emph on +matrix exponential +\emph default + of +\begin_inset Formula $\xihat$ +\end_inset + +: +\begin_inset Formula +\[ +T(t)=e^{t\xihat}\define\lim_{n\rightarrow\infty}\left(I+\frac{t}{n}\xihat\right)^{n}=\sum_{k=0}^{\infty}\frac{t^{k}}{k!}\xihat^{k} +\] + +\end_inset + +We call this mapping from 2D twists matrices +\begin_inset Formula $\xihat$ +\end_inset + + to 2D rigid transformations the +\emph on +exponential map. +\end_layout + +\begin_layout Standard +The above has all elements of Lie group theory. + We call the space of 2D rigid transformations, along with the composition + operation, the +\emph on +special Euclidean group +\emph default + +\begin_inset Formula $\SEtwo$ +\end_inset + +. + It is called a Lie group because it is simultaneously a topological group + and a manifold, which implies that the multiplication and the inversion + operations are smooth. + The space of 2D twists, together with a special binary operation to be + defined below, is called the Lie algebra +\begin_inset Formula $\setwo$ +\end_inset + + associated with +\begin_inset Formula $\SEtwo$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +Basic Lie Group Concepts +\end_layout + +\begin_layout Standard +We now define the concepts illustrated above, introduce some notation, and + see what we can say in general. + After this we then introduce the most commonly used Lie groups and their + Lie algebras. +\end_layout + +\begin_layout Subsection +A Manifold and a Group +\end_layout + +\begin_layout Standard +A +\series bold +Lie group +\series default + +\begin_inset Formula $G$ +\end_inset + + is both a group +\emph on +and +\emph default + a manifold that possesses a smooth group operation. + Associated with it is a +\series bold +Lie Algebra +\series default + +\begin_inset Formula $\gg$ +\end_inset + + which, loosely speaking, can be identified with the tangent space at the + identity and completely defines how the groups behaves around the identity. + There is a mapping from +\begin_inset Formula $\gg$ +\end_inset + + back to +\begin_inset Formula $G$ +\end_inset + +, called the +\series bold +exponential map +\series default + +\begin_inset Formula +\[ +\exp:\gg\rightarrow G +\] + +\end_inset + +which is typically a many-to-one mapping. + The corresponding inverse can be define locally around the origin and hence + is a +\begin_inset Quotes eld +\end_inset + +logarithm +\begin_inset Quotes erd +\end_inset + + +\begin_inset Formula +\[ +\log:G\rightarrow\gg +\] + +\end_inset + +that maps elements in a neighborhood of +\begin_inset Formula $id$ +\end_inset + + in G to an element in +\begin_inset Formula $\gg$ +\end_inset + +. +\end_layout + +\begin_layout Standard +An important family of Lie groups are the matrix Lie groups, whose elements + are +\begin_inset Formula $n\times n$ +\end_inset + + invertible matrices. + The set of all such matrices, together with the matrix multiplication, + is called the general linear group +\begin_inset Formula $GL(n)$ +\end_inset + + of dimension +\begin_inset Formula $n$ +\end_inset + +, and any closed subgroup of it is a +\series bold + matrix Lie group +\series default +. + Most if not all Lie groups we are interested in will be matrix Lie groups. +\end_layout + +\begin_layout Subsection +Lie Algebra +\end_layout + +\begin_layout Standard +The Lie Algebra +\begin_inset Formula $\gg$ +\end_inset + + is called an algebra because it is endowed with a binary operation, the + +\series bold +Lie bracket +\series default + +\begin_inset Formula $[X,Y]$ +\end_inset + +, the properties of which are closely related to the group operation of + +\begin_inset Formula $G$ +\end_inset + +. + For example, for algebras associated with matrix Lie groups, the Lie bracket + is given by +\begin_inset Formula $[A,B]\define AB-BA$ +\end_inset + +. +\end_layout + +\begin_layout Standard +The relationship of the Lie bracket to the group operation is as follows: + for commutative Lie groups vector addition +\begin_inset Formula $X+Y$ +\end_inset + + in +\begin_inset Formula $\gg$ +\end_inset + + mimicks the group operation. + For example, if we have +\begin_inset Formula $Z=X+Y$ +\end_inset + + in +\begin_inset Formula $\gg$ +\end_inset + +, when mapped backed to +\begin_inset Formula $G$ +\end_inset + + via the exponential map we obtain +\begin_inset Formula +\[ +e^{Z}=e^{X+Y}=e^{X}e^{Y} +\] + +\end_inset + +However, this does +\emph on +not +\emph default + hold for non-commutative Lie groups: +\begin_inset Formula +\[ +Z=\log(e^{X}e^{Y})\neq X+Y +\] + +\end_inset + +Instead, +\begin_inset Formula $Z$ +\end_inset + + can be calculated using the Baker-Campbell-Hausdorff (BCH) formula +\begin_inset CommandInset citation +LatexCommand cite +key "Hall00book" + +\end_inset + + +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +http://en.wikipedia.org/wiki/Baker–Campbell–Hausdorff_formula +\end_layout + +\end_inset + +: +\begin_inset Formula +\[ +Z=X+Y+[X,Y]/2+[X-Y,[X,Y]]/12-[Y,[X,[X,Y]]]/24+\ldots +\] + +\end_inset + +For commutative groups the bracket is zero and we recover +\begin_inset Formula $Z=X+Y$ +\end_inset + +. + For non-commutative groups we can use the BCH formula to approximate it. +\end_layout + +\begin_layout Subsection +Exponential Coordinates +\end_layout + +\begin_layout Standard +For +\begin_inset Formula $n$ +\end_inset + +-dimensional matrix Lie groups, as a vector space the Lie algebra +\begin_inset Formula $\gg$ +\end_inset + + is isomorphic to +\begin_inset Formula $\mathbb{R}^{n}$ +\end_inset + +, and we can define the hat operator +\begin_inset CommandInset citation +LatexCommand cite +after "page 41" +key "Murray94book" + +\end_inset + +, +\begin_inset Formula +\[ +\hat{}:x\in\mathbb{R}^{n}\rightarrow\xhat\in\gg +\] + +\end_inset + +which maps +\begin_inset Formula $n$ +\end_inset + +-vectors +\begin_inset Formula $x\in\mathbb{R}^{n}$ +\end_inset + + to elements of +\begin_inset Formula $\gg$ +\end_inset + +. + In the case of matrix Lie groups, the elements +\begin_inset Formula $\xhat$ +\end_inset + + of +\begin_inset Formula $\gg$ +\end_inset + + are also +\begin_inset Formula $n\times n$ +\end_inset + + matrices, and the map is given by +\begin_inset Formula +\begin{equation} +\xhat=\sum_{i=1}^{n}x_{i}G^{i}\label{eq:generators} +\end{equation} + +\end_inset + +where the +\begin_inset Formula $G^{i}$ +\end_inset + + are +\begin_inset Formula $n\times n$ +\end_inset + + matrices known as Lie group generators. + The meaning of the map +\begin_inset Formula $x\rightarrow\xhat$ +\end_inset + + will depend on the group +\begin_inset Formula $G$ +\end_inset + + and will generally have an intuitive interpretation. +\end_layout + +\begin_layout Subsection +Actions +\end_layout + +\begin_layout Standard +An important concept is that of a group element acting on an element of + a manifold +\begin_inset Formula $M$ +\end_inset + +. + For example, 2D rotations act on 2D points, 3D rotations act on 3D points, + etc. + In particular, a +\series bold +left action +\series default + of +\begin_inset Formula $G$ +\end_inset + + on +\begin_inset Formula $M$ +\end_inset + + is defined as a smooth map +\begin_inset Formula $\Phi:G\times M\rightarrow M$ +\end_inset + + such that +\begin_inset CommandInset citation +LatexCommand cite +after "Appendix A" +key "Murray94book" + +\end_inset + +: +\end_layout + +\begin_layout Enumerate +The identity element +\begin_inset Formula $e$ +\end_inset + + has no effect, i.e., +\begin_inset Formula $\Phi(e,p)=p$ +\end_inset + + +\end_layout + +\begin_layout Enumerate +Composing two actions can be combined into one action: +\begin_inset Formula $\Phi(g,\Phi(h,p))=\Phi(gh,p)$ +\end_inset + + +\end_layout + +\begin_layout Standard +The (usual) action of an +\begin_inset Formula $n$ +\end_inset + +-dimensional matrix group +\begin_inset Formula $G$ +\end_inset + + is matrix-vector multiplication on +\begin_inset Formula $\mathbb{R}^{n}$ +\end_inset + +, +\begin_inset Formula +\[ +q=Ap +\] + +\end_inset + +with +\begin_inset Formula $p,q\in\mathbb{R}^{n}$ +\end_inset + + and +\begin_inset Formula $A\in G\subseteq GL(n)$ +\end_inset + +. + +\end_layout + +\begin_layout Subsection +The Adjoint Map and Adjoint Representation +\end_layout + +\begin_layout Standard +Suppose a point +\begin_inset Formula $p$ +\end_inset + + is specified as +\begin_inset Formula $p'$ +\end_inset + + in the frame +\begin_inset Formula $T$ +\end_inset + +, i.e., +\begin_inset Formula $p'=Tp$ +\end_inset + +, where +\begin_inset Formula $T$ +\end_inset + + transforms from the global coordinates +\begin_inset Formula $p$ +\end_inset + + to the local frame +\begin_inset Formula $p'$ +\end_inset + +. + To apply an action +\begin_inset Formula $A$ +\end_inset + + we first need to undo +\begin_inset Formula $T$ +\end_inset + +, then apply +\begin_inset Formula $A$ +\end_inset + +, and then transform the result back to +\begin_inset Formula $T$ +\end_inset + +: +\begin_inset Formula +\[ +q'=TAT^{-1}p' +\] + +\end_inset + +The matrix +\begin_inset Formula $TAT^{-1}$ +\end_inset + + is said to be conjugate to +\begin_inset Formula $A$ +\end_inset + +, and this is a central element of group theory. +\end_layout + +\begin_layout Standard +In general, the +\series bold +adjoint map +\series default + +\begin_inset Formula $\AAdd g$ +\end_inset + + maps a group element +\begin_inset Formula $a\in G$ +\end_inset + + to its +\series bold +conjugate +\series default + +\begin_inset Formula $gag^{-1}$ +\end_inset + + by +\begin_inset Formula $g$ +\end_inset + +. + This map captures conjugacy in the group +\begin_inset Formula $G$ +\end_inset + +, but there is an equivalent notion in the Lie algebra +\begin_inset Formula $\mathfrak{\gg}$ +\end_inset + +, +\begin_inset Note Note +status open + +\begin_layout Plain Layout +http://en.wikipedia.org/wiki/Exponential_map +\end_layout + +\end_inset + + +\begin_inset Formula +\[ +\AAdd ge^{\xhat}=g\exp\left(\xhat\right)g^{-1}=\exp(\Ad g{\xhat}) +\] + +\end_inset + +where +\begin_inset Formula $\Ad g:\gg\rightarrow\mathfrak{\gg}$ +\end_inset + + is a map parameterized by a group element +\begin_inset Formula $g$ +\end_inset + +, and is called the +\emph on +adjoint representation +\emph default +. + The intuitive explanation is that a change +\begin_inset Formula $\exp\left(\xhat\right)$ +\end_inset + + defined around the origin, but applied at the group element +\begin_inset Formula $g$ +\end_inset + +, can be written in one step by taking the adjoint +\begin_inset Formula $\Ad g{\xhat}$ +\end_inset + + of +\begin_inset Formula $\xhat$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +In the special case of matrix Lie groups the adjoint can be written as +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +http://en.wikipedia.org/wiki/Adjoint_representation_of_a_Lie_group +\end_layout + +\end_inset + + +\begin_inset Formula +\[ +\Ad T{\xhat}\define T\xhat T^{-1} +\] + +\end_inset + +and hence we have +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{equation} +Te^{\xhat}T^{-1}=e^{T\xhat T^{-1}}\label{eq:matrixAdjoint} +\end{equation} + +\end_inset + +where both +\begin_inset Formula $T\in G$ +\end_inset + + and +\begin_inset Formula $\xhat\in\gg$ +\end_inset + + are +\begin_inset Formula $n\times n$ +\end_inset + + matrices for an +\begin_inset Formula $n$ +\end_inset + +-dimensional Lie group. +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +2D Rotations +\end_layout + +\begin_layout Standard +We first look at a very simple group, the 2D rotations. +\end_layout + +\begin_layout Subsection +Basics +\end_layout + +\begin_layout Standard +The Lie group +\begin_inset Formula $\SOtwo$ +\end_inset + + is a subgroup of the general linear group +\begin_inset Formula $GL(2)$ +\end_inset + + of +\begin_inset Formula $2\times2$ +\end_inset + + invertible matrices. + Its Lie algebra +\begin_inset Formula $\sotwo$ +\end_inset + + is the vector space of +\begin_inset Formula $2\times2$ +\end_inset + + skew-symmetric matrices. + Since +\begin_inset Formula $\SOtwo$ +\end_inset + + is a one-dimensional manifold, +\begin_inset Formula $\sotwo$ +\end_inset + + is isomorphic to +\begin_inset Formula $\mathbb{R}$ +\end_inset + + and we define +\begin_inset Formula +\[ +\hat{}:\mathbb{R}\rightarrow\sotwo +\] + +\end_inset + + +\begin_inset Formula +\[ +\hat{}:\omega\rightarrow\what=\skew{\omega} +\] + +\end_inset + +which maps the angle +\begin_inset Formula $\omega$ +\end_inset + + to the +\begin_inset Formula $2\times2$ +\end_inset + + skew-symmetric matrix +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none + +\begin_inset Formula $\skew{\omega}$ +\end_inset + +: +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit + +\begin_inset Formula +\[ +\skew{\omega}=\left[\begin{array}{cc} +0 & -\omega\\ +\omega & 0 +\end{array}\right] +\] + +\end_inset + +The exponential map can be computed in closed form as +\begin_inset Formula +\[ +e^{\skew{\omega}}=\left[\begin{array}{cc} +\cos\omega & -\sin\omega\\ +\sin\omega & \cos\omega +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +\begin_inset CommandInset label +LatexCommand label +name "sub:Diagonalized2D" + +\end_inset + +Diagonalized Form +\end_layout + +\begin_layout Standard +The matrix +\begin_inset Formula $\skew 1$ +\end_inset + + can be diagonalized (see +\begin_inset CommandInset citation +LatexCommand cite +key "Hall00book" + +\end_inset + +) with eigenvalues +\begin_inset Formula $-i$ +\end_inset + + and +\begin_inset Formula $i$ +\end_inset + + , and eigenvectors +\begin_inset Formula $\left[\begin{array}{c} +1\\ +i +\end{array}\right]$ +\end_inset + + and +\begin_inset Formula $\left[\begin{array}{c} +i\\ +1 +\end{array}\right]$ +\end_inset + + . + Readers familiar with projective geometry will recognize these as the circular + points when promoted to homogeneous coordinates. + In particular: +\begin_inset Formula +\[ +\skew{\omega}=\left[\begin{array}{cc} +0 & -\omega\\ +\omega & 0 +\end{array}\right]=\left[\begin{array}{cc} +1 & i\\ +i & 1 +\end{array}\right]\left[\begin{array}{cc} +-i\omega & 0\\ +0 & i\omega +\end{array}\right]\left[\begin{array}{cc} +1 & i\\ +i & 1 +\end{array}\right]^{-1} +\] + +\end_inset + +and hence +\begin_inset Formula +\[ +e^{\skew{\omega}}=\frac{1}{2}\left[\begin{array}{cc} +1 & i\\ +i & 1 +\end{array}\right]\left[\begin{array}{cc} +e^{-i\omega} & 0\\ +0 & e^{i\omega} +\end{array}\right]\left[\begin{array}{cc} +1 & -i\\ +-i & 1 +\end{array}\right]=\left[\begin{array}{cc} +\cos\omega & -\sin\omega\\ +\sin\omega & \cos\omega +\end{array}\right] +\] + +\end_inset + +where the latter can be shown using +\begin_inset Formula $e^{i\omega}=\cos\omega+i\sin\omega$ +\end_inset + +. +\end_layout + +\begin_layout Subsection +Adjoint +\end_layout + +\begin_layout Standard +The adjoint for +\begin_inset Formula $\sotwo$ +\end_inset + + is trivially equal to the identity, as is the case for +\emph on +all +\emph default + commutative groups: +\begin_inset Formula +\begin{eqnarray*} +\Ad R\what & = & \left[\begin{array}{cc} +\cos\theta & -\sin\theta\\ +\sin\theta & \cos\theta +\end{array}\right]\left[\begin{array}{cc} +0 & -\omega\\ +\omega & 0 +\end{array}\right]\left[\begin{array}{cc} +\cos\theta & -\sin\theta\\ +\sin\theta & \cos\theta +\end{array}\right]^{T}\\ + & = & \omega\left[\begin{array}{cc} +-\sin\theta & -\cos\theta\\ +\cos\theta & -\sin\theta +\end{array}\right]\left[\begin{array}{cc} +\cos\theta & \sin\theta\\ +-\sin\theta & \cos\theta +\end{array}\right]=\left[\begin{array}{cc} +0 & -\omega\\ +\omega & 0 +\end{array}\right] +\end{eqnarray*} + +\end_inset + +i.e., +\begin_inset Formula +\[ +\Ad R\what=\what +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Actions +\end_layout + +\begin_layout Standard +In the case of +\begin_inset Formula $\SOtwo$ +\end_inset + + the vector space is +\begin_inset Formula $\Rtwo$ +\end_inset + +, and the group action corresponds to rotating a point +\begin_inset Formula +\[ +q=Rp +\] + +\end_inset + +We would now like to know what an incremental rotation parameterized by + +\begin_inset Formula $\omega$ +\end_inset + + would do: +\begin_inset Formula +\[ +q(\text{\omega})=Re^{\skew{\omega}}p +\] + +\end_inset + +For small angles +\begin_inset Formula $\omega$ +\end_inset + + we have +\begin_inset Formula +\[ +e^{\skew{\omega}}\approx\skew{\omega}=\omega\skew 1 +\] + +\end_inset + +where +\begin_inset Formula $\skew 1$ +\end_inset + + acts like a restricted +\begin_inset Quotes eld +\end_inset + +cross product +\begin_inset Quotes erd +\end_inset + + in the plane on points: +\begin_inset Formula +\begin{equation} +\skew 1\left[\begin{array}{c} +x\\ +y +\end{array}\right]=R_{\pi/2}\left[\begin{array}{c} +x\\ +y +\end{array}\right]=\left[\begin{array}{c} +-y\\ +x +\end{array}\right]\label{eq:RestrictedCross} +\end{equation} + +\end_inset + +Hence the derivative of the action is given as +\begin_inset Formula +\[ +\deriv{q(\omega)}{\omega}=R\deriv{}{\omega}\left(e^{\skew{\omega}}p\right)=R\deriv{}{\omega}\left(\omega\skew 1p\right)=RH_{p} +\] + +\end_inset + +where +\begin_inset Formula $H_{p}$ +\end_inset + + is a +\begin_inset Formula $2\times1$ +\end_inset + + matrix that depends on +\begin_inset Formula $p$ +\end_inset + +: +\begin_inset Formula +\[ +H_{p}\define\skew 1p=\left[\begin{array}{c} +-p_{y}\\ +p_{x} +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +2D Rigid Transformations +\end_layout + +\begin_layout Subsection +Basics +\end_layout + +\begin_layout Standard +The Lie group +\begin_inset Formula $\SEtwo$ +\end_inset + + is a subgroup of the general linear group +\begin_inset Formula $GL(3)$ +\end_inset + + of +\begin_inset Formula $3\times3$ +\end_inset + + invertible matrices of the form +\begin_inset Formula +\[ +T\define\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + +where +\begin_inset Formula $R\in\SOtwo$ +\end_inset + + is a rotation matrix and +\begin_inset Formula $t\in\Rtwo$ +\end_inset + + is a translation vector. + +\begin_inset Formula $\SEtwo$ +\end_inset + + is the +\emph on +semi-direct product +\emph default + of +\begin_inset Formula $\Rtwo$ +\end_inset + + by +\begin_inset Formula $SO(2)$ +\end_inset + +, written as +\begin_inset Formula $\SEtwo=\Rtwo\rtimes\SOtwo$ +\end_inset + +. + In particular, any element +\begin_inset Formula $T$ +\end_inset + + of +\begin_inset Formula $\SEtwo$ +\end_inset + + can be written as +\begin_inset Formula +\[ +T=\left[\begin{array}{cc} +0 & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +R & 0\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + +and they compose as +\begin_inset Formula +\[ +T_{1}T_{2}=\left[\begin{array}{cc} +R_{1} & t_{1}\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +R_{2} & t_{2}\\ +0 & 1 +\end{array}\right]=\left[\begin{array}{cc} +R_{1}R_{2} & R_{1}t_{2}+t_{1}\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + +Hence, an alternative way of writing down elements of +\begin_inset Formula $\SEtwo$ +\end_inset + + is as the ordered pair +\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}) +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +The corresponding Lie algebra +\begin_inset Formula $\setwo$ +\end_inset + + is the vector space of +\begin_inset Formula $3\times3$ +\end_inset + + twists +\begin_inset Formula $\xihat$ +\end_inset + + parameterized by the +\emph on +twist coordinates +\emph default + +\begin_inset Formula $\xi\in\Rthree$ +\end_inset + +, with the mapping +\begin_inset Formula +\[ +\xi\define\left[\begin{array}{c} +v\\ +\omega +\end{array}\right]\rightarrow\xihat\define\left[\begin{array}{cc} +\skew{\omega} & v\\ +0 & 0 +\end{array}\right] +\] + +\end_inset + +Note we think of robots as having a pose +\begin_inset Formula $(x,y,\theta)$ +\end_inset + + and hence I reserved the first two components for translation and the last + for rotation. + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none +The corresponding Lie group generators are +\begin_inset Formula +\[ +G^{x}=\left[\begin{array}{ccc} +0 & 0 & 1\\ +0 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{y}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & 1\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{\theta}=\left[\begin{array}{ccc} +0 & -1 & 0\\ +1 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right] +\] + +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit +Applying the exponential map to a twist +\begin_inset Formula $\xi$ +\end_inset + + yields a screw motion yielding an element in +\begin_inset Formula $\SEtwo$ +\end_inset + +: +\begin_inset Formula +\[ +T=e^{\xihat}=\left(e^{\skew{\omega}},(I-e^{\skew{\omega}})\frac{v^{\perp}}{\omega}\right) +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +The Adjoint Map +\end_layout + +\begin_layout Standard +The adjoint is +\begin_inset Formula +\begin{eqnarray} +\Ad T{\xihat} & = & T\xihat T^{-1}\nonumber \\ + & = & \left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +\skew{\omega} & v\\ +0 & 0 +\end{array}\right]\left[\begin{array}{cc} +R^{T} & -R^{T}t\\ +0 & 1 +\end{array}\right]\nonumber \\ + & = & \left[\begin{array}{cc} +\skew{\omega} & -\skew{\omega}t+Rv\\ +0 & 0 +\end{array}\right]\nonumber \\ + & = & \left[\begin{array}{cc} +\skew{\omega} & Rv-\omega R_{\pi/2}t\\ +0 & 0 +\end{array}\right]\label{eq:adjointSE2} +\end{eqnarray} + +\end_inset + +From this we can express the Adjoint map in terms of plane twist coordinates: +\begin_inset Formula +\[ +\left[\begin{array}{c} +v'\\ +\omega' +\end{array}\right]=\left[\begin{array}{cc} +R & -R_{\pi/2}t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{c} +v\\ +\omega +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Actions +\end_layout + +\begin_layout Standard +The action of +\begin_inset Formula $\SEtwo$ +\end_inset + + on 2D points is done by embedding the points in +\begin_inset Formula $\mathbb{R}^{3}$ +\end_inset + + by using homogeneous coordinates +\begin_inset Formula +\[ +\hat{q}=\left[\begin{array}{c} +q\\ +1 +\end{array}\right]=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{c} +p\\ +1 +\end{array}\right]=T\hat{p} +\] + +\end_inset + +Analoguous to +\begin_inset Formula $\SEthree$ +\end_inset + +, we can compute a velocity +\begin_inset Formula $\xihat\hat{p}$ +\end_inset + + in the local +\begin_inset Formula $T$ +\end_inset + + frame: +\begin_inset Formula +\[ +\xihat\hat{p}=\left[\begin{array}{cc} +\skew{\omega} & v\\ +0 & 0 +\end{array}\right]\left[\begin{array}{c} +p\\ +1 +\end{array}\right]=\left[\begin{array}{c} +\skew{\omega}p+v\\ +0 +\end{array}\right] +\] + +\end_inset + +By only taking the top two rows, we can write this as a velocity in +\begin_inset Formula $\Rtwo$ +\end_inset + +, as the product of a +\begin_inset Formula $2\times3$ +\end_inset + + matrix +\begin_inset Formula $H_{p}$ +\end_inset + + that acts upon the exponential coordinates +\begin_inset Formula $\xi$ +\end_inset + + directly: +\begin_inset Formula +\[ +\skew{\omega}p+v=v+R_{\pi/2}p\omega=\left[\begin{array}{cc} +I_{2} & R_{\pi/2}p\end{array}\right]\left[\begin{array}{c} +v\\ +\omega +\end{array}\right]=H_{p}\xi +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +3D Rotations +\end_layout + +\begin_layout Subsection +Basics +\end_layout + +\begin_layout Standard +The Lie group +\begin_inset Formula $\SOthree$ +\end_inset + + is a subgroup of the general linear group +\begin_inset Formula $GL(3)$ +\end_inset + + of +\begin_inset Formula $3\times3$ +\end_inset + + invertible matrices. + Its Lie algebra +\begin_inset Formula $\sothree$ +\end_inset + + is the vector space of +\begin_inset Formula $3\times3$ +\end_inset + + skew-symmetric matrices +\begin_inset Formula $\what$ +\end_inset + +. + Since +\begin_inset Formula $\SOthree$ +\end_inset + + is a three-dimensional manifold, +\begin_inset Formula $\sothree$ +\end_inset + + is isomorphic to +\begin_inset Formula $\Rthree$ +\end_inset + + and we define the map +\begin_inset Formula +\[ +\hat{}:\Rthree\rightarrow\sothree +\] + +\end_inset + + +\begin_inset Formula +\[ +\hat{}:\omega\rightarrow\what=\Skew{\omega} +\] + +\end_inset + +which maps 3-vectors +\begin_inset Formula $\omega$ +\end_inset + + to skew-symmetric matrices +\begin_inset Formula $\Skew{\omega}$ +\end_inset + + : +\begin_inset Formula +\[ +\Skew{\omega}=\left[\begin{array}{ccc} +0 & -\omega_{z} & \omega_{y}\\ +\omega_{z} & 0 & -\omega_{x}\\ +-\omega_{y} & \omega_{x} & 0 +\end{array}\right]=\omega_{x}G^{x}+\omega_{y}G^{y}+\omega_{z}G^{z} +\] + +\end_inset + +Here the matrices +\begin_inset Formula $G^{i}$ +\end_inset + + are the generators for +\begin_inset Formula $\SOthree$ +\end_inset + +, +\begin_inset Formula +\[ +G^{x}=\left(\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & -1\\ +0 & 1 & 0 +\end{array}\right)\mbox{}G^{y}=\left(\begin{array}{ccc} +0 & 0 & 1\\ +0 & 0 & 0\\ +-1 & 0 & 0 +\end{array}\right)\mbox{ }G^{z}=\left(\begin{array}{ccc} +0 & -1 & 0\\ +1 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right) +\] + +\end_inset + +corresponding to a rotation around +\begin_inset Formula $X$ +\end_inset + +, +\begin_inset Formula $Y$ +\end_inset + +, and +\begin_inset Formula $Z$ +\end_inset + +, respectively. + The Lie bracket +\begin_inset Formula $[x,y]$ +\end_inset + + in +\begin_inset Formula $\sothree$ +\end_inset + + corresponds to the cross product +\begin_inset Formula $x\times y$ +\end_inset + + in +\begin_inset Formula $\Rthree$ +\end_inset + +. +\end_layout + +\begin_layout Standard +Hence, for every +\begin_inset Formula $3$ +\end_inset + +-vector +\begin_inset Formula $\omega$ +\end_inset + + there is a corresponding rotation matrix +\begin_inset Formula +\[ +R=e^{\Skew{\omega}} +\] + +\end_inset + +which defines a canonical parameterization of +\begin_inset Formula $\SOthree$ +\end_inset + +, with +\begin_inset Formula $\omega$ +\end_inset + + known as the canonical or exponential coordinates. + It is equivalent to the axis-angle representation for rotations, where + the unit vector +\begin_inset Formula $\omega/\theta$ +\end_inset + + defines the rotation axis, and its magnitude the amount of rotation +\begin_inset Formula $\theta$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +The exponential map can be computed in closed form using +\series bold +Rodrigues' formula +\series default + +\begin_inset CommandInset citation +LatexCommand cite +after "page 28" +key "Murray94book" + +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{equation} +e^{\what}=I+\frac{\sin\theta}{\theta}\what+\frac{1\text{−}\cos\theta}{\theta^{2}}\what^{2}\label{eq:Rodrigues} +\end{equation} + +\end_inset + +where +\begin_inset Formula $\what^{2}=\omega\omega^{T}-I$ +\end_inset + +, with +\begin_inset Formula $\omega\omega^{T}$ +\end_inset + + the outer product of +\begin_inset Formula $\omega$ +\end_inset + +. + Hence, a slightly more efficient variant is +\begin_inset Formula +\begin{equation} +e^{\what}=\left(\cos\theta\right)I+\frac{\sin\theta}{\theta}\what+\frac{1\text{−}cos\theta}{\theta^{2}}\omega\omega^{T}\label{eq:Rodrigues2} +\end{equation} + +\end_inset + + +\end_layout + +\begin_layout Subsection +Diagonalized Form +\end_layout + +\begin_layout Standard +Because a 3D rotation +\begin_inset Formula $R$ +\end_inset + + leaves the axis +\begin_inset Formula $\omega$ +\end_inset + + unchanged, +\begin_inset Formula $R$ +\end_inset + + can be diagonalized as +\begin_inset Formula +\[ +R=C\left(\begin{array}{ccc} +e^{-i\theta} & 0 & 0\\ +0 & e^{i\theta} & 0\\ +0 & 0 & 1 +\end{array}\right)C^{-1} +\] + +\end_inset + +with +\begin_inset Formula $C=\left(\begin{array}{ccc} +c_{1} & c_{2} & \omega/\theta\end{array}\right)$ +\end_inset + +, where +\begin_inset Formula $c_{1}$ +\end_inset + + and +\begin_inset Formula $c_{2}$ +\end_inset + + are the complex eigenvectors corresponding to the 2D rotation around +\begin_inset Formula $\omega$ +\end_inset + +. + This also means that, by +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:matrixAdjoint" + +\end_inset + +, +\begin_inset Formula +\[ +\hat{\omega}=C\left(\begin{array}{ccc} +-i\theta & 0 & 0\\ +0 & i\theta & 0\\ +0 & 0 & 0 +\end{array}\right)C^{-1} +\] + +\end_inset + +In this case, +\begin_inset Formula $C$ +\end_inset + + has complex columns, but we also have +\begin_inset Formula +\begin{equation} +\hat{\omega}=B\left(\begin{array}{ccc} +0 & -\theta & 0\\ +\theta & 0 & 0\\ +0 & 0 & 0 +\end{array}\right)B^{T}\label{eq:OmegaDecomposed} +\end{equation} + +\end_inset + +with +\begin_inset Formula $B=\left(\begin{array}{ccc} +b_{1} & b_{2} & \omega/\theta\end{array}\right)$ +\end_inset + +, where +\begin_inset Formula $b_{1}$ +\end_inset + + and +\begin_inset Formula $b_{2}$ +\end_inset + + form a basis for the 2D plane through the origin and perpendicular to +\begin_inset Formula $\omega$ +\end_inset + +. + Clearly, from Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sub:Diagonalized2D" + +\end_inset + +, we have +\begin_inset Formula +\[ +c_{1}=B\left(\begin{array}{c} +1\\ +i\\ +0 +\end{array}\right)\mbox{\,\,\,\ and\,\,\,\,\,}c_{2}=B\left(\begin{array}{c} +i\\ +1\\ +0 +\end{array}\right) +\] + +\end_inset + +and when we exponentiate +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:OmegaDecomposed" + +\end_inset + + we expose the 2D rotation around the axis +\begin_inset Formula $\omega/\theta$ +\end_inset + + with magnitude +\begin_inset Formula $\theta$ +\end_inset + +: +\begin_inset Formula +\[ +R=B\left(\begin{array}{ccc} +\cos\theta & -\sin\theta & 0\\ +\sin\theta & \cos\theta & 0\\ +0 & 0 & 1 +\end{array}\right)B^{T} +\] + +\end_inset + +The latter form for +\begin_inset Formula $R$ +\end_inset + + can be used to prove Rodrigues' formula. + Expanding the above, we get +\begin_inset Formula +\[ +R=\left(\cos\theta\right)\left(b_{1}b_{1}^{T}+b_{2}b_{2}^{T}\right)+\left(\sin\theta\right)\left(b_{2}b_{1}^{T}-b_{1}b_{2}^{T}\right)+\omega\omega^{T}/\theta^{2} +\] + +\end_inset + + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none +\begin_inset Formula +\begin{eqnarray*} +R & = & \left(\begin{array}{ccc} +b_{1} & b_{2} & \omega/\theta\end{array}\right)\left(\begin{array}{ccc} +\cos\theta & -\sin\theta & 0\\ +\sin\theta & \cos\theta & 0\\ +0 & 0 & 1 +\end{array}\right)\left(\begin{array}{c} +b_{1}^{T}\\ +b_{2}^{T}\\ +\omega^{T}/\theta +\end{array}\right)\\ + & = & \left(\begin{array}{ccc} +b_{1} & b_{2} & \omega/\theta\end{array}\right)\left(\begin{array}{c} +b_{1}^{T}\cos\theta-b_{2}^{T}\sin\theta\\ +b_{1}^{T}\sin\theta+b_{2}^{T}\cos\theta\\ +\omega^{T}/\theta +\end{array}\right)\\ + & = & b_{1}b_{1}^{T}\cos\theta-b_{1}b_{2}^{T}\sin\theta+b_{2}b_{1}^{T}\sin\theta+b_{2}b_{2}^{T}\cos\theta+\omega\omega^{T}/\theta^{2} +\end{eqnarray*} + +\end_inset + + +\end_layout + +\end_inset + +Because +\begin_inset Formula $B$ +\end_inset + + is a rotation matrix, we have +\begin_inset Formula $BB^{T}=b_{1}b_{1}^{T}+b_{2}b_{2}^{T}+\omega\omega^{T}/\theta^{2}=I$ +\end_inset + +, and using +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:OmegaDecomposed" + +\end_inset + + it is easy to show that +\begin_inset Formula $b_{2}b_{1}^{T}-b_{1}b_{2}^{T}=\hat{\omega}/\theta$ +\end_inset + +, hence +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit + +\begin_inset Formula +\[ +R=\left(\cos\theta\right)(I-\omega\omega^{T}/\theta^{2})+\left(\sin\theta\right)\left(\hat{\omega}/\theta\right)+\omega\omega^{T}/\theta^{2} +\] + +\end_inset + +which is equivalent to +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:Rodrigues2" + +\end_inset + +. +\end_layout + +\begin_layout Subsection +The Adjoint Map +\end_layout + +\begin_layout Standard +For rotation matrices +\begin_inset Formula $R$ +\end_inset + + we can prove the following identity (see +\begin_inset CommandInset ref +LatexCommand vref +reference "proof1" + +\end_inset + +): +\begin_inset Formula +\begin{equation} +R\Skew{\omega}R^{T}=\Skew{R\omega}\label{eq:property1} +\end{equation} + +\end_inset + +Hence, given property +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:property1" + +\end_inset + +, the adjoint map for +\begin_inset Formula $\sothree$ +\end_inset + + simplifies to +\begin_inset Formula +\[ +\Ad R{\Skew{\omega}}=R\Skew{\omega}R^{T}=\Skew{R\omega} +\] + +\end_inset + +and this can be expressed in exponential coordinates simply by rotating + the axis +\begin_inset Formula $\omega$ +\end_inset + + to +\begin_inset Formula $R\omega$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +As an example, to apply an axis-angle rotation +\begin_inset Formula $\omega$ +\end_inset + + to a point +\begin_inset Formula $p$ +\end_inset + + in the frame +\begin_inset Formula $R$ +\end_inset + +, we could: +\end_layout + +\begin_layout Enumerate +First transform +\begin_inset Formula $p$ +\end_inset + + back to the world frame, apply +\begin_inset Formula $\omega$ +\end_inset + +, and then rotate back: +\begin_inset Formula +\[ +q=Re^{\Skew{\omega}}R^{T} +\] + +\end_inset + + +\end_layout + +\begin_layout Enumerate +Immediately apply the transformed axis-angle transformation +\begin_inset Formula $\Ad R{\Skew{\omega}}=\Skew{R\omega}$ +\end_inset + +: +\begin_inset Formula +\[ +q=e^{\Skew{R\omega}}p +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Actions +\end_layout + +\begin_layout Standard +In the case of +\begin_inset Formula $\SOthree$ +\end_inset + + the vector space is +\begin_inset Formula $\Rthree$ +\end_inset + +, and the group action corresponds to rotating a point +\begin_inset Formula +\[ +q=Rp +\] + +\end_inset + +We would now like to know what an incremental rotation parameterized by + +\begin_inset Formula $\omega$ +\end_inset + + would do: +\begin_inset Formula +\[ +q(\omega)=Re^{\Skew{\omega}}p +\] + +\end_inset + +hence the derivative is: +\begin_inset Formula +\[ +\deriv{q(\omega)}{\omega}=R\deriv{}{\omega}\left(e^{\Skew{\omega}}p\right)=R\deriv{}{\omega}\left(\Skew{\omega}p\right)=R\Skew{-p} +\] + +\end_inset + +To show the last equality note that +\begin_inset Formula +\[ +\Skew{\omega}p=\omega\times p=-p\times\omega=\Skew{-p}\omega +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +3D Rigid Transformations +\end_layout + +\begin_layout Standard +The Lie group +\begin_inset Formula $\SEthree$ +\end_inset + + is a subgroup of the general linear group +\begin_inset Formula $GL(4)$ +\end_inset + + 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 & 1 +\end{array}\right] +\] + +\end_inset + +where +\begin_inset Formula $R\in\SOthree$ +\end_inset + + is a rotation matrix and +\begin_inset Formula $t\in\Rthree$ +\end_inset + + is a translation vector. + An alternative way of writing down elements of +\begin_inset Formula $\SEthree$ +\end_inset + + is as the ordered pair +\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}) +\] + +\end_inset + + Its Lie algebra +\begin_inset Formula $\sethree$ +\end_inset + + is the vector space of +\begin_inset Formula $4\times4$ +\end_inset + + twists +\begin_inset Formula $\xihat$ +\end_inset + + parameterized by the +\emph on +twist coordinates +\emph default + +\begin_inset Formula $\xi\in\Rsix$ +\end_inset + +, with the mapping +\begin_inset CommandInset citation +LatexCommand cite +key "Murray94book" + +\end_inset + + +\begin_inset Formula +\[ +\xi\define\left[\begin{array}{c} +\omega\\ +v +\end{array}\right]\rightarrow\xihat\define\left[\begin{array}{cc} +\Skew{\omega} & v\\ +0 & 0 +\end{array}\right] +\] + +\end_inset + +Note we follow Frank Park's convention and reserve the first three components + for rotation, and the last three for translation. + Hence, with this parameterization, the generators for +\begin_inset Formula $\SEthree$ +\end_inset + + are +\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) +\] + +\end_inset + +Applying the exponential map to a twist +\begin_inset Formula $\xi$ +\end_inset + + yields a screw motion yielding an element in +\begin_inset Formula $\SEthree$ +\end_inset + +: +\begin_inset Formula +\[ +T=\exp\xihat +\] + +\end_inset + +A closed form solution for the exponential map is given in +\begin_inset CommandInset citation +LatexCommand cite +after "page 42" +key "Murray94book" + +\end_inset + +. +\end_layout + +\begin_layout Standard + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none +\begin_inset Formula +\[ +\exp\left(\left[\begin{array}{c} +\omega\\ +v +\end{array}\right]t\right)=\left[\begin{array}{cc} +e^{\Skew{\omega}t} & (I-e^{\Skew{\omega}t})\left(\omega\times v\right)+\omega\omega^{T}vt\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +The Adjoint Map +\end_layout + +\begin_layout Standard +The adjoint is +\begin_inset Formula +\begin{eqnarray*} +\Ad T{\xihat} & = & T\xihat T^{-1}\\ + & = & \left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +\Skew{\omega} & v\\ +0 & 0 +\end{array}\right]\left[\begin{array}{cc} +R^{T} & -R^{T}t\\ +0 & 1 +\end{array}\right]\\ + & = & \left[\begin{array}{cc} +\Skew{R\omega} & -\Skew{R\omega}t+Rv\\ +0 & 0 +\end{array}\right]\\ + & = & \left[\begin{array}{cc} +\Skew{R\omega} & t\times R\omega+Rv\\ +0 & 0 +\end{array}\right] +\end{eqnarray*} + +\end_inset + +From this we can express the Adjoint map in terms of twist coordinates (see + also +\begin_inset CommandInset citation +LatexCommand cite +key "Murray94book" + +\end_inset + + and FP): +\begin_inset Formula +\[ +\left[\begin{array}{c} +\omega'\\ +v' +\end{array}\right]=\left[\begin{array}{cc} +R & 0\\ +\Skew tR & R +\end{array}\right]\left[\begin{array}{c} +\omega\\ +v +\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\\ +1 +\end{array}\right]=\left[\begin{array}{c} +Rp+t\\ +1 +\end{array}\right]=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{c} +p\\ +1 +\end{array}\right]=T\hat{p} +\] + +\end_inset + +We would now like to know what an incremental rotation parameterized by + +\begin_inset Formula $\xi$ +\end_inset + + would do: +\begin_inset Formula +\[ +\hat{q}(\xi)=Te^{\xihat}\hat{p} +\] + +\end_inset + +hence the derivative is +\begin_inset Formula +\[ +\deriv{\hat{q}(\xi)}{\xi}=T\deriv{}{\xi}\left(\xihat\hat{p}\right) +\] + +\end_inset + +where +\begin_inset Formula $\xihat\hat{p}$ +\end_inset + + corresponds to a velocity in +\begin_inset Formula $\mathbb{R}^{4}$ +\end_inset + + (in the local +\begin_inset Formula $T$ +\end_inset + + frame): +\begin_inset Formula +\[ +\xihat\hat{p}=\left[\begin{array}{cc} +\Skew{\omega} & v\\ +0 & 0 +\end{array}\right]\left[\begin{array}{c} +p\\ +1 +\end{array}\right]=\left[\begin{array}{c} +\omega\times p+v\\ +0 +\end{array}\right] +\] + +\end_inset + +Notice how velocities are analogous to points at infinity in projective + geometry: they correspond to free vectors indicating a direction and magnitude + of change. +\end_layout + +\begin_layout Standard +By only taking the top three rows, we can write this as a velocity in +\begin_inset Formula $\Rthree$ +\end_inset + +, as the product of a +\begin_inset Formula $3\times6$ +\end_inset + + matrix +\begin_inset Formula $H_{p}$ +\end_inset + + that acts upon the exponential coordinates +\begin_inset Formula $\xi$ +\end_inset + + directly: +\begin_inset Formula +\[ +\omega\times p+v=-p\times\omega+v=\left[\begin{array}{cc} +-\Skew p & I_{3}\end{array}\right]\left[\begin{array}{c} +\omega\\ +v +\end{array}\right] +\] + +\end_inset + +The inverse action +\begin_inset Formula $T^{-1}p$ +\end_inset + + is +\begin_inset Formula +\[ +\hat{q}=\left[\begin{array}{c} +q\\ +1 +\end{array}\right]=\left[\begin{array}{c} +R^{T}(p-t)\\ +1 +\end{array}\right]=\left[\begin{array}{cc} +R^{T} & -R^{T}t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{c} +p\\ +1 +\end{array}\right]=T^{-1}\hat{p} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +2D Affine Transformations +\end_layout + +\begin_layout Standard +The Lie group +\begin_inset Formula $Aff(2)$ +\end_inset + + is a subgroup of the general linear group +\begin_inset Formula $GL(3)$ +\end_inset + + of +\begin_inset Formula $3\times3$ +\end_inset + + invertible matrices that maps the line infinity to itself, and hence preserves + paralellism. + The affine transformation matrices +\begin_inset Formula $A$ +\end_inset + + can be written as +\begin_inset CommandInset citation +LatexCommand cite +key "Mei08tro" + +\end_inset + + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none + +\begin_inset Formula +\[ +\left[\begin{array}{ccc} +m_{11} & m_{12} & t_{1}\\ +m_{21} & m_{22} & t_{2}\\ +0 & 0 & k +\end{array}\right] +\] + +\end_inset + +with +\begin_inset Formula $M\in GL(2)$ +\end_inset + +, +\begin_inset Formula $t\in\Rtwo$ +\end_inset + +, and +\begin_inset Formula $k$ +\end_inset + + a scalar chosen such that +\begin_inset Formula $det(A)=1$ +\end_inset + +. + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit +Note that just as +\begin_inset Formula $\SEtwo$ +\end_inset + + is a semi-direct product, so too is +\begin_inset Formula $Aff(2)=\Rtwo\rtimes GL(2)$ +\end_inset + +. + In particular, any affine transformation +\begin_inset Formula $A$ +\end_inset + + can be written as +\begin_inset Formula +\[ +A=\left[\begin{array}{cc} +0 & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +M & 0\\ +0 & k +\end{array}\right] +\] + +\end_inset + +and they compose as +\begin_inset Formula +\[ +A_{1}A_{2}=\left[\begin{array}{cc} +M_{1} & t_{1}\\ +0 & k_{1} +\end{array}\right]\left[\begin{array}{cc} +M_{2} & t_{2}\\ +0 & k_{2} +\end{array}\right]=\left[\begin{array}{cc} +M_{1}M_{2} & M_{2}t_{2}+k_{2}t_{1}\\ +0 & k_{1}k_{2} +\end{array}\right] +\] + +\end_inset + +From this it can be gleaned that the groups +\begin_inset Formula $\SOtwo$ +\end_inset + + and +\begin_inset Formula $\SEtwo$ +\end_inset + + are both subgroups, with +\begin_inset Formula $\SOtwo\subset\SEtwo\subset\Afftwo$ +\end_inset + +. + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none +By choosing the generators carefully we maintain this hierarchy among the + associated Lie algebras. + In particular, +\begin_inset Formula $\setwo$ +\end_inset + + +\begin_inset Formula +\[ +G^{1}=\left[\begin{array}{ccc} +0 & 0 & 1\\ +0 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{2}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & 1\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{3}=\left[\begin{array}{ccc} +0 & -1 & 0\\ +1 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right] +\] + +\end_inset + +can be extended to the +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit +Lie algebra +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none + +\begin_inset Formula $\afftwo$ +\end_inset + + using the three additional generators +\begin_inset Formula +\[ +G^{4}=\left[\begin{array}{ccc} +0 & 1 & 0\\ +1 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{5}=\left[\begin{array}{ccc} +1 & 0 & 0\\ +0 & -1 & 0\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{6}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & -1 & 0\\ +0 & 0 & 1 +\end{array}\right] +\] + +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit +Hence, the Lie algebra +\begin_inset Formula $\afftwo$ +\end_inset + + is the vector space of +\begin_inset Formula $3\times3$ +\end_inset + + incremental affine transformations +\begin_inset Formula $\ahat$ +\end_inset + + parameterized by 6 parameters +\begin_inset Formula $\aa\in\mathbb{R}^{6}$ +\end_inset + +, with the mapping +\begin_inset Formula +\[ +\aa\rightarrow\ahat\define\left[\begin{array}{ccc} +a_{5} & a_{4}-a_{3} & a_{1}\\ +a_{4}+a_{3} & -a_{5}-a_{6} & a_{2}\\ +0 & 0 & a_{6} +\end{array}\right] +\] + +\end_inset + +Note that +\begin_inset Formula $G_{5}$ +\end_inset + + and +\begin_inset Formula $G_{6}$ +\end_inset + + change the relative scale of +\begin_inset Formula $x$ +\end_inset + + and +\begin_inset Formula $y$ +\end_inset + + but without changing the determinant: +\begin_inset Formula +\[ +e^{xG_{5}}=\exp\left[\begin{array}{ccc} +x & 0 & 0\\ +0 & -x & 0\\ +0 & 0 & 0 +\end{array}\right]=\left[\begin{array}{ccc} +e^{x} & 0 & 0\\ +0 & 1/e^{x} & 0\\ +0 & 0 & 1 +\end{array}\right] +\] + +\end_inset + + +\begin_inset Formula +\[ +e^{xG_{6}}=\exp\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & -x & 0\\ +0 & 0 & x +\end{array}\right]=\left[\begin{array}{ccc} +1 & 0 & 0\\ +0 & 1/e^{x} & 0\\ +0 & 0 & e^{x} +\end{array}\right] +\] + +\end_inset + +It might be nicer to have the correspondence with scaling +\begin_inset Formula $x$ +\end_inset + + and +\begin_inset Formula $y$ +\end_inset + + more direct, by choosing +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none + +\begin_inset Formula +\[ +\mbox{ }G^{5}=\left[\begin{array}{ccc} +1 & 0 & 0\\ +0 & 0 & 0\\ +0 & 0 & -1 +\end{array}\right]\mbox{ }G^{6}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & 1 & 0\\ +0 & 0 & -1 +\end{array}\right] +\] + +\end_inset + +and hence +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit + +\begin_inset Formula +\[ +e^{xG_{5}}=\exp\left[\begin{array}{ccc} +x & 0 & 0\\ +0 & 0 & 0\\ +0 & 0 & -x +\end{array}\right]=\left[\begin{array}{ccc} +e^{x} & 0 & 0\\ +0 & 1 & 0\\ +0 & 0 & 1/e^{x} +\end{array}\right] +\] + +\end_inset + + +\begin_inset Formula +\[ +e^{xG_{6}}=\exp\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & x & 0\\ +0 & 0 & -x +\end{array}\right]=\left[\begin{array}{ccc} +1 & 0 & 0\\ +0 & e^{x} & 0\\ +0 & 0 & 1/e^{x} +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Section +2D Homographies +\end_layout + +\begin_layout Standard +When viewed as operations on images, represented by 2D projective space + +\begin_inset Formula $\mathcal{P}^{3}$ +\end_inset + +, 3D rotations are a special case of 2D homographies. + These are now treated, loosely based on the exposition in +\begin_inset CommandInset citation +LatexCommand cite +key "Mei06iros,Mei08tro" + +\end_inset + +. +\end_layout + +\begin_layout Subsection +Basics +\end_layout + +\begin_layout Standard +The Lie group +\begin_inset Formula $\SLthree$ +\end_inset + + is a subgroup of the general linear group +\begin_inset Formula $GL(3)$ +\end_inset + + of +\begin_inset Formula $3\times3$ +\end_inset + + invertible matrices with determinant +\begin_inset Formula $1$ +\end_inset + +. + The homographies generalize transformations of the 2D projective space, + and +\begin_inset Formula $\Afftwo\subset\SLthree$ +\end_inset + +. + +\end_layout + +\begin_layout Standard + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none +We can extend +\begin_inset Formula $\afftwo$ +\end_inset + + to the Lie algebra +\begin_inset Formula $\slthree$ +\end_inset + + by adding two generators +\begin_inset Formula +\[ +G^{7}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & 0\\ +1 & 0 & 0 +\end{array}\right]\mbox{ }G^{8}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & 0\\ +0 & 1 & 0 +\end{array}\right] +\] + +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit +obtaining the vector space of +\begin_inset Formula $3\times3$ +\end_inset + + incremental homographies +\begin_inset Formula $\hhat$ +\end_inset + + parameterized by 8 parameters +\begin_inset Formula $\hh\in\mathbb{R}^{8}$ +\end_inset + +, with the mapping +\begin_inset Formula +\[ +h\rightarrow\hhat\define\left[\begin{array}{ccc} +h_{5} & h_{4}-h_{3} & h_{1}\\ +h_{4}+h_{3} & -h_{5}-h_{6} & h_{2}\\ +h_{7} & h_{8} & h_{6} +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Tensor Notation +\end_layout + +\begin_layout Itemize +A homography between 2D projective spaces +\begin_inset Formula $A$ +\end_inset + + and +\begin_inset Formula $B$ +\end_inset + + can be written in tensor notation +\begin_inset Formula $H_{A}^{B}$ +\end_inset + + +\end_layout + +\begin_layout Itemize +Applying a homography is then a tensor contraction +\begin_inset Formula $x^{B}=H_{A}^{B}x^{A}$ +\end_inset + +, mapping points in +\begin_inset Formula $A$ +\end_inset + + to points in +\begin_inset Formula $B$ +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +The inverse of a homography can be found by contracting with two permutation + tensors: +\begin_inset Formula +\[ +H_{B}^{A}=H_{A_{1}}^{B_{1}}H_{A_{2}}^{B_{2}}\epsilon_{B_{1}B_{2}B}\epsilon^{A_{1}A_{2}A} +\] + +\end_inset + + +\end_layout + +\end_inset + + +\begin_inset Note Note +status collapsed + +\begin_layout Subsection +The Adjoint Map +\end_layout + +\begin_layout Plain Layout +The adjoint can be done using tensor notation. + Denoting an incremental homography in space +\begin_inset Formula $A$ +\end_inset + + as +\begin_inset Formula $\hhat_{A_{1}}^{A_{2}}$ +\end_inset + +, we have, for example for +\begin_inset Formula $G_{1}$ +\end_inset + + +\begin_inset Formula +\begin{eqnarray*} +\hhat_{B_{1}}^{B_{2}}=\Ad{H_{A}^{B}}{\hhat_{A_{1}}^{A_{2}}} & = & H_{A_{2}}^{B_{2}}\hhat_{A_{1}}^{A_{2}}H_{B_{1}}^{A_{1}}\\ + & = & H_{A_{2}}^{B_{2}}\left[\begin{array}{ccc} +0 & 0 & 1\\ +0 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right]H_{A_{2}}^{B_{2}}H_{A_{3}}^{B_{3}}\epsilon_{B_{1}B_{2}B_{3}}\epsilon^{A_{1}A_{2}A_{3}}\\ + & = & H_{1}^{B_{2}}H_{A_{2}}^{B_{2}}H_{A_{3}}^{B_{3}}\epsilon_{B_{1}B_{2}B_{3}}\epsilon^{3A_{2}A_{3}} +\end{eqnarray*} + +\end_inset + +This does not seem to help. +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section* +Appendix: Proof of Property +\begin_inset CommandInset ref +LatexCommand ref +reference "proof1" + +\end_inset + + +\end_layout + +\begin_layout Standard +We can prove the following identity for rotation matrices +\begin_inset Formula $R$ +\end_inset + +, +\begin_inset Formula +\begin{eqnarray} +R\Skew{\omega}R^{T} & = & R\Skew{\omega}\left[\begin{array}{ccc} +a_{1} & a_{2} & a_{3}\end{array}\right]\nonumber \\ + & = & R\left[\begin{array}{ccc} +\omega\times a_{1} & \omega\times a_{2} & \omega\times a_{3}\end{array}\right]\nonumber \\ + & = & \left[\begin{array}{ccc} +a_{1}(\omega\times a_{1}) & a_{1}(\omega\times a_{2}) & a_{1}(\omega\times a_{3})\\ +a_{2}(\omega\times a_{1}) & a_{2}(\omega\times a_{2}) & a_{2}(\omega\times a_{3})\\ +a_{3}(\omega\times a_{1}) & a_{3}(\omega\times a_{2}) & a_{3}(\omega\times a_{3}) +\end{array}\right]\nonumber \\ + & = & \left[\begin{array}{ccc} +\omega(a_{1}\times a_{1}) & \omega(a_{2}\times a_{1}) & \omega(a_{3}\times a_{1})\\ +\omega(a_{1}\times a_{2}) & \omega(a_{2}\times a_{2}) & \omega(a_{3}\times a_{2})\\ +\omega(a_{1}\times a_{3}) & \omega(a_{2}\times a_{3}) & \omega(a_{3}\times a_{3}) +\end{array}\right]\nonumber \\ + & = & \left[\begin{array}{ccc} +0 & -\omega a_{3} & \omega a_{2}\\ +\omega a_{3} & 0 & -\omega a_{1}\\ +-\omega a_{2} & \omega a_{1} & 0 +\end{array}\right]\nonumber \\ + & = & \Skew{R\omega}\label{proof1} +\end{eqnarray} + +\end_inset + +where +\begin_inset Formula $a_{1}$ +\end_inset + +, +\begin_inset Formula $a_{2}$ +\end_inset + +, and +\begin_inset Formula $a_{3}$ +\end_inset + + are the +\emph on +rows +\emph default + of +\begin_inset Formula $R$ +\end_inset + +. + Above we made use of the orthogonality of rotation matrices and the triple + product rule: +\begin_inset Formula +\[ +a(b\times c)=b(c\times a)=c(a\times b) +\] + +\end_inset + +Similarly, without proof +\begin_inset CommandInset citation +LatexCommand cite +after "Lemma 2.3" +key "Murray94book" + +\end_inset + +: +\begin_inset Formula +\[ +R(a\times b)=Ra\times Rb +\] + +\end_inset + + +\end_layout + +\begin_layout Section* +Appendix: Alternative Generators for +\begin_inset Formula $\slthree$ +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset CommandInset citation +LatexCommand cite +key "Mei06iros" + +\end_inset + + uses the following generators for +\begin_inset Formula $\slthree$ +\end_inset + +: +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none + +\begin_inset Formula +\[ +G^{1}=\left[\begin{array}{ccc} +0 & 0 & 1\\ +0 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{2}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & 1\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{3}=\left[\begin{array}{ccc} +0 & 1 & 0\\ +0 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right] +\] + +\end_inset + + +\begin_inset Formula +\[ +G^{4}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +1 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{5}=\left[\begin{array}{ccc} +1 & 0 & 0\\ +0 & -1 & 0\\ +0 & 0 & 0 +\end{array}\right]\mbox{ }G^{6}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & -1 & 0\\ +0 & 0 & 1 +\end{array}\right] +\] + +\end_inset + + +\begin_inset Formula +\[ +G^{7}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & 0\\ +1 & 0 & 0 +\end{array}\right]\mbox{ }G^{8}=\left[\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & 0\\ +0 & 1 & 0 +\end{array}\right] +\] + +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit +We choose to use a different linear combination as the basis. +\end_layout + +\begin_layout Standard +\begin_inset CommandInset bibtex +LatexCommand bibtex +bibfiles "../../../papers/refs" +options "plain" + +\end_inset + + +\end_layout + +\end_body +\end_document diff --git a/doc/LieGroups.pdf b/doc/LieGroups.pdf new file mode 100644 index 000000000..f271f35c0 Binary files /dev/null and b/doc/LieGroups.pdf differ diff --git a/doc/cholesky.lyx b/doc/cholesky.lyx new file mode 100644 index 000000000..99157fadc --- /dev/null +++ b/doc/cholesky.lyx @@ -0,0 +1,170 @@ +#LyX 1.6.7 created this file. For more info see http://www.lyx.org/ +\lyxformat 345 +\begin_document +\begin_header +\textclass article +\use_default_options true +\language english +\inputencoding auto +\font_roman default +\font_sans default +\font_typewriter default +\font_default_family default +\font_sc false +\font_osf false +\font_sf_scale 100 +\font_tt_scale 100 + +\graphics default +\paperfontsize default +\use_hyperref false +\papersize default +\use_geometry false +\use_amsmath 1 +\use_esint 1 +\cite_engine basic +\use_bibtopic false +\paperorientation portrait +\secnumdepth 3 +\tocdepth 3 +\paragraph_separation indent +\defskip medskip +\quotes_language english +\papercolumns 1 +\papersides 1 +\paperpagestyle default +\tracking_changes false +\output_changes false +\author "" +\author "" +\end_header + +\begin_body + +\begin_layout Section +Basic solving with Cholesky +\end_layout + +\begin_layout Standard +Solving a linear least-squares system: +\begin_inset Formula \[ +\arg\min_{x}\left\Vert Ax-b\right\Vert ^{2}\] + +\end_inset + +Set derivative equal to zero: +\begin_inset Formula \begin{align*} +0 & =2A^{T}\left(Ax-b\right)\\ +0 & =A^{T}Ax-A^{T}b\end{align*} + +\end_inset + +For comparison, with QR we do +\begin_inset Formula \begin{align*} +0 & =R^{T}Q^{T}QRx-R^{T}Qb\\ + & =R^{T}Rx-R^{T}Qb\\ +Rx & =Qb\\ +x & =R^{-1}Qb\end{align*} + +\end_inset + +But with Cholesky we do +\begin_inset Formula \begin{align*} +0 & =R^{T}RR^{T}Rx-R^{T}Rb\\ + & =R^{T}Rx-b\\ + & =Rx-R^{-T}b\\ +x & =R^{-1}R^{-T}b\end{align*} + +\end_inset + + +\end_layout + +\begin_layout Section +Frontal (rank-deficient) solving with Cholesky +\end_layout + +\begin_layout Standard +To do multi-frontal elimination, we decompose into rank-deficient conditionals. + +\begin_inset Formula \[ +\left[\begin{array}{cccccc} +\cdot & \cdot & \cdot & \cdot & \cdot & \cdot\\ +\cdot & \cdot & \cdot & \cdot & \cdot & \cdot\\ +\cdot & \cdot & \cdot & \cdot & \cdot & \cdot\end{array}\right]\to\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula \[ +\left[\begin{array}{cc} +R^{T} & 0\\ +S^{T} & C^{T}\end{array}\right]\left[\begin{array}{cc} +R & S\\ +0 & C\end{array}\right]=\left[\begin{array}{cc} +F^{T}F & F^{T}G\\ +G^{T}F & G^{T}G\end{array}\right]\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset space ~ +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula \[ +R^{T}R=F^{T}F\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset space ~ +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula \begin{align*} +R^{T}S & =F^{T}G\\ +S & =R^{-T}F^{T}G\end{align*} + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset space ~ +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula \begin{align*} +S^{T}S+C^{T}C & =G^{T}G\\ +G^{T}FR^{-1}R^{-T}F^{T}G+C^{T}C & =G^{T}G\\ +G^{T}QRR^{-1}R^{-T}R^{T}Q^{T}G+C^{T}C & =G^{T}G\\ +\textbf{if }R\textbf{ is invertible, }G^{T}G+C^{T}C & =G^{T}G\\ +C^{T}C & =0\end{align*} + +\end_inset + + +\end_layout + +\end_body +\end_document diff --git a/doc/images/circular.pdf b/doc/images/circular.pdf new file mode 100644 index 000000000..b0f43f649 Binary files /dev/null and b/doc/images/circular.pdf differ diff --git a/doc/images/circular.png b/doc/images/circular.png new file mode 100644 index 000000000..71c346a8e Binary files /dev/null and b/doc/images/circular.png differ diff --git a/doc/images/gtsam-structure.graffle b/doc/images/gtsam-structure.graffle new file mode 100644 index 000000000..22eb72dc3 --- /dev/null +++ b/doc/images/gtsam-structure.graffle @@ -0,0 +1,1302 @@ + + + + + ActiveLayerIndex + 0 + ApplicationVersion + + com.omnigroup.OmniGraffle + 138.14.0.129428 + + AutoAdjust + + BackgroundGraphic + + Bounds + {{0, 0}, {559.29, 782.89}} + Class + SolidGraphic + ID + 2 + Style + + fill + + GradientColor + + w + 0.666667 + + + shadow + + Draws + NO + + stroke + + Draws + NO + + + + CanvasOrigin + {0, 0} + CanvasSize + {559.29, 782.89} + ColumnAlign + 1 + ColumnSpacing + 36 + CreationDate + 2010-07-13 00:07:47 -0400 + Creator + Frank Dellaert + DisplayScale + 1 0/72 in = 1 0/72 in + FileType + flat + GraphDocumentVersion + 6 + GraphicsList + + + Class + LineGraphic + Head + + ID + 19 + + ID + 35 + Points + + {191.649, 522.933} + {191.649, 558.208} + + Style + + stroke + + HeadArrow + FilledArrow + LineType + 1 + TailArrow + 0 + + + Tail + + ID + 34 + + + + Bounds + {{156.649, 558.708}, {70, 27.449}} + Class + ShapedGraphic + ID + 19 + Shape + Rectangle + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1038\cocoasubrtf320 +{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\qc\pardirnatural + +\f0\fs24 \cf0 tests} + + + + Class + Group + Graphics + + + Class + LineGraphic + Head + + ID + 30 + + ID + 22 + Points + + {220.827, 437.595} + {201.125, 473.266} + + Style + + stroke + + HeadArrow + FilledArrow + LineType + 1 + TailArrow + 0 + + + Tail + + ID + 25 + + + + Class + LineGraphic + Head + + ID + 28 + + ID + 23 + Points + + {198.977, 245.588} + {219.976, 281.282} + + Style + + stroke + + HeadArrow + FilledArrow + LineType + 1 + TailArrow + 0 + + + Tail + + ID + 33 + + + + Class + LineGraphic + Head + + ID + 25 + + ID + 24 + Points + + {234.015, 373.654} + {229.931, 409.211} + + Style + + stroke + + HeadArrow + FilledArrow + LineType + 1 + TailArrow + 0 + + + Tail + + ID + 27 + + + + Bounds + {{193.649, 409.708}, {70, 27.449}} + Class + ShapedGraphic + ID + 25 + Shape + Rectangle + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1038\cocoasubrtf320 +{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\qc\pardirnatural + +\f0\fs24 \cf0 nonlinear} + + + + Class + LineGraphic + Head + + ID + 27 + + ID + 26 + Points + + {230.126, 309.655} + {233.82, 345.213} + + Style + + stroke + + HeadArrow + FilledArrow + LineType + 1 + TailArrow + 0 + + + Tail + + ID + 28 + + + + Bounds + {{200.649, 345.708}, {70, 27.449}} + Class + ShapedGraphic + ID + 27 + Shape + Rectangle + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1038\cocoasubrtf320 +{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\qc\pardirnatural + +\f0\fs24 \cf0 linear} + + + + Bounds + {{193.649, 281.708}, {70, 27.449}} + Class + ShapedGraphic + ID + 28 + Shape + Rectangle + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1038\cocoasubrtf320 +{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\qc\pardirnatural + +\f0\fs24 \cf0 inference} + + + + Class + LineGraphic + Head + + ID + 30 + + ID + 29 + Points + + {152.743, 373.628} + {188.492, 473.239} + + Style + + stroke + + HeadArrow + FilledArrow + LineType + 1 + TailArrow + 0 + + + Tail + + ID + 32 + + + + Bounds + {{158.649, 473.708}, {70, 27.449}} + Class + ShapedGraphic + ID + 30 + Shape + Rectangle + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1038\cocoasubrtf320 +{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\qc\pardirnatural + +\f0\fs24 \cf0 slam} + + + + Class + LineGraphic + Head + + ID + 32 + + ID + 31 + Points + + {185.872, 245.631} + {152.363, 345.234} + + Style + + stroke + + HeadArrow + FilledArrow + LineType + 1 + TailArrow + 0 + + + Tail + + ID + 33 + + + + Bounds + {{112.649, 345.708}, {70, 27.449}} + Class + ShapedGraphic + ID + 32 + Shape + Rectangle + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1038\cocoasubrtf320 +{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\qc\pardirnatural + +\f0\fs24 \cf0 geometry} + + + + Bounds + {{155.649, 217.708}, {70, 27.449}} + Class + ShapedGraphic + ID + 33 + Shape + Rectangle + Text + + Text + {\rtf1\ansi\ansicpg1252\cocoartf1038\cocoasubrtf320 +{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\qc\pardirnatural + +\f0\fs24 \cf0 base} + + + + ID + 21 + + + Bounds + {{86.649, 196.433}, {210, 326}} + Class + ShapedGraphic + ID + 34 + Shape + Rectangle + Style + + Text + + Align + 0 + Text + {\rtf1\ansi\ansicpg1252\cocoartf1038\cocoasubrtf320 +{\fonttbl\f0\fswiss\fcharset0 Helvetica;} +{\colortbl;\red255\green255\blue255;} +\pard\tx560\tx1120\tx1680\tx2240\tx2800\tx3360\tx3920\tx4480\tx5040\tx5600\tx6160\tx6720\pardirnatural + +\f0\fs24 \cf0 gtsam} + + TextPlacement + 0 + + + GridInfo + + GuidesLocked + NO + GuidesVisible + YES + HPages + 1 + ImageCounter + 3 + KeepToScale + + Layers + + + Lock + NO + Name + Layer 1 + Print + YES + View + YES + + + LayoutInfo + + Animate + NO + AutoLayout + 2 + LineLength + 0.4643835723400116 + circoMinDist + 18 + circoSeparation + 0.0 + layoutEngine + dot + neatoSeparation + 0.0 + twopiSeparation + 0.0 + + LinksVisible + NO + MagnetsVisible + NO + MasterSheets + + ModificationDate + 2010-07-13 00:17:24 -0400 + Modifier + Frank Dellaert + NotesVisible + NO + Orientation + 2 + OriginVisible + NO + OutlineStyle + Basic + PageBreaks + NO + PrintInfo + + NSBottomMargin + + float + 41 + + NSLeftMargin + + float + 18 + + NSPaperSize + + size + {595.29, 841.89} + + NSRightMargin + + float + 18 + + NSTopMargin + + float + 18 + + + PrintOnePage + + QuickLookPreview + + JVBERi0xLjMKJcTl8uXrp/Og0MTGCjUgMCBvYmoKPDwgL0xlbmd0aCA2IDAgUiAvRmls + dGVyIC9GbGF0ZURlY29kZSA+PgpzdHJlYW0KeAGlV8tuWzcQ3d+v4NJemCZn+NzWbYF6 + 1dQCuii6cAW5kSHZjeQW6N/3DHl5TclXUoNYSOQwnPeZM8Mv6pP6ogw+3mcVE6ndSv2q + XtTt3d6q5V7Z8tkv1Y3RXhnN2RjrglzXVCR0yhAantTtz6vdcvXX29+PG7VbQy0xFdXs + kopeB5dyVuS8dj6q5Vbd/rS16vvX4kKuV71R1jlczcoTaZusH+pNmrlpbLnJ2ZWbo865 + my6WmxTCJZ2Jyk3n0yWdKR9YP+PnqJPZXNKJ5EjsiHvGz9tfVpvHt/U/q7vXzetuvV29 + 7dZLSXUKkCrJDaYkl6xRTEGK+YQC3is7PNeS3j2Ukhj1cIcK2fKPG/lC7aWSSPakzeag + HbOq2rzO+JFSP6BiwAcJPowCNjpFNteaOOJW5u8Wytbq3uD7xhqDv1xUi+1w+6PVqLha + PKnf1NWf14IyUldv10AavveP7WR7rX5Xi3v1w6LaPsSm9TVtnqOOTF5Foyhqh5zEoeSg + 87APtQmShaBJB4KSvINIh+NITamV9/ZUpKyNNdEph1hVH+tw9UeLbApxvzofox2BafJX + xjgKMrD11TGOfXO6muRPxzjVE5HVwr62X7bTUav17t8+/Jl6SX/Z5HWKRIVFAluncIbi + ewtkMuM/vUlq0+pWUCLkVlBqPekcgHHHGRULaRDhpDkA484FbRJaZoMzZDoGj7MIzHtR + +LmcHkjD9BMU3+PP8zDjr7TSZJId2oHBh51J5qCTwKMzyezQhY6GA5OdtCDSlg/QKN4L + XRDo4hj3U+8PVj33HXqI/6rARb6MDXR6H6fNXI3nidCPOx15DDHHj/hHa2+uh4qJqQEO + evxMQqNjpxgeB8QtCQUowCdpcIF1JuNPAiBFAQgptmhZzqkIo8bRepxlnRNGGqoBKmRC + ERi0kH2gCoAZ6QkAJxM8Cg0uCkvAtvg7mnQBgDTRdiaVC05n5hFzzWQn/TD0ABhrUOon + xBcQBH7SRH8TDICZWRiUtmqlFBwJDc6qOSDDI9onGoEEjzHdRekRGAhTHf1Fc2BYN1p4 + ab88jRNg1VCymwhjurM8T5hkKj+7Rpgfgjo7Gpr4RJsfxPsBMcyMQgIfSXcekqfRxgAG + C3RvPxSRnYg1SSZFPxWvNuuX1ePuWi2e6+w70Rckq4VklxFt8NC0lfVLxxziIMTIwZ/s + C2LpJZCTEKMPkYswpgXYqZBgYicsRZR1soRGAzGSdaLwM8x8lL7YF01oEGpjotibFH+N + 98LFzWQh9xBLDJ3JTnq2L0rxvr0vCoS+qS8Y6/J7X8xjoHSIR4jH68LVy+vLBxyA1mWu + He1uhPlhrOfKj94J3xBjyQqgRc5Zc/RCZ2Mv1wE5jAOygMhFXMS2DhwIuRaFiF9xYm0j + VRwkzEpjy1nysfJjkx466f+BA+BWTDoTBQf5wCT8jTYGDEPhl2oSZ4ap8mMz2UvXAYmN + 9xzdJXB+AEliJfIpyRYBOIPq8S6hiK5xmcckEcaCsdg4vGGN+77cjdo76waPORKClZlB + 2LI44hZ40OaxM2Zkz2ZEaHMySFHeF2gyOasG4ZzVSIVMjNEgRacplpmHvmgGmyw2nAv5 + kC2FyOiEFnRYn30GPnBksO5bizPMo2AMowaiiQxpA5ooI5RSKPVCzNi8I86w0OAxJt5J + thJaWMZqdCC8QhWjNAYr4qjSZxNSvGsmZQgGwwWVzaTDg854EwUizaSsAdZhBvYmVSdd + UyJbVNmh5PGEJxceO8c7VB0Q5f00j6a641UFHsvYuf16bkS01xJeWEIPou5obMryHtF+ + PScM9bnUNudpMu7byf7CKl3cLg+1FAZ5Z2PjQNGrNxj/mB+ahe+OiKIwjuSsynogJ6CB + sD2JsDzbkx+EMcqZLFKyFUBPd4ZNekb6LArqiiLvSvHNh+rbu8ne38lkF8O7yaGTFhR8 + +g8FPYzxCmVuZHN0cmVhbQplbmRvYmoKNiAwIG9iagoxNDU4CmVuZG9iagozIDAgb2Jq + Cjw8IC9UeXBlIC9QYWdlIC9QYXJlbnQgNCAwIFIgL1Jlc291cmNlcyA3IDAgUiAvQ29u + dGVudHMgNSAwIFIgL01lZGlhQm94IFswIDAgNTU5LjI5IDc4Mi44OV0KPj4KZW5kb2Jq + CjcgMCBvYmoKPDwgL1Byb2NTZXQgWyAvUERGIC9UZXh0IC9JbWFnZUIgL0ltYWdlQyAv + SW1hZ2VJIF0gL0NvbG9yU3BhY2UgPDwgL0NzMSA4IDAgUgovQ3MyIDEzIDAgUiA+PiAv + Rm9udCA8PCAvRjEuMCAxNCAwIFIgPj4gL1hPYmplY3QgPDwgL0ltMSA5IDAgUiAvSW0y + IDExIDAgUgo+PiA+PgplbmRvYmoKOSAwIG9iago8PCAvTGVuZ3RoIDEwIDAgUiAvVHlw + ZSAvWE9iamVjdCAvU3VidHlwZSAvSW1hZ2UgL1dpZHRoIDQ2NCAvSGVpZ2h0IDY5NiAv + SW50ZXJwb2xhdGUKdHJ1ZSAvQ29sb3JTcGFjZSAxNSAwIFIgL0ludGVudCAvUGVyY2Vw + dHVhbCAvU01hc2sgMTYgMCBSIC9CaXRzUGVyQ29tcG9uZW50CjggL0ZpbHRlciAvRmxh + dGVEZWNvZGUgPj4Kc3RyZWFtCngB7dCBAAAAAMOg+VNf4QCFUGHAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwY + MGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAED + BgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDA + gAEDBgwYMGDAgAEDBgwYMGDAgAEDBgwYMGDAwDswyVIAAQplbmRzdHJlYW0KZW5kb2Jq + CjEwIDAgb2JqCjQyNDgKZW5kb2JqCjExIDAgb2JqCjw8IC9MZW5ndGggMTIgMCBSIC9U + eXBlIC9YT2JqZWN0IC9TdWJ0eXBlIC9JbWFnZSAvV2lkdGggMTg0IC9IZWlnaHQgMTAw + IC9JbnRlcnBvbGF0ZQp0cnVlIC9Db2xvclNwYWNlIDE1IDAgUiAvSW50ZW50IC9QZXJj + ZXB0dWFsIC9TTWFzayAxOCAwIFIgL0JpdHNQZXJDb21wb25lbnQKOCAvRmlsdGVyIC9G + bGF0ZURlY29kZSA+PgpzdHJlYW0KeAHt0IEAAAAAw6D5Ux/khVBhwIABAwYMGDBgwIAB + AwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBg + wIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYM + GDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIAB + AwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBgwIABAwYMGDBg + wIABAwYMGDBgwIABAwYMGDBgwIABA48DA9egAAEKZW5kc3RyZWFtCmVuZG9iagoxMiAw + IG9iagoyNjMKZW5kb2JqCjE2IDAgb2JqCjw8IC9MZW5ndGggMTcgMCBSIC9UeXBlIC9Y + T2JqZWN0IC9TdWJ0eXBlIC9JbWFnZSAvV2lkdGggNDY0IC9IZWlnaHQgNjk2IC9Db2xv + clNwYWNlCi9EZXZpY2VHcmF5IC9JbnRlcnBvbGF0ZSB0cnVlIC9CaXRzUGVyQ29tcG9u + ZW50IDggL0ZpbHRlciAvRmxhdGVEZWNvZGUgPj4Kc3RyZWFtCngB7drtT1TnogVwlXdm + GGZwGGCAgsPrgEinYEdACwSColBfsdgKQUdNsSDVOJEUtRiqRKIorQQ1ipSIBi0BQ9QY + Nedfu89Gc0+ruHrOp+5173o+nJxk9STL9euz9wbPmjU6WkALaAEtoAW0gBbQAlrg/8MC + a3Vsu8B/9e+f+VOs+/eJ0bHRAv92WWeY/gPWd5bmTxAbG6dj0wViYw2QRft3pCua7yjj + ExIS350kHZss8B4kISHe/ItmUP9G9L1mXFy8oUxKTnY4HE6nM0XHNgsYDoOSnJyUmGiZ + YlGLM8Y8ZQ2moXSmuFypbrdHx1YLuN2pLleKUU02pO9EP/HQXeE0d9PSTHGlejxp673e + 9HSfL0PHJgv4fOnpXu/6NI8n1ZViiZo7ah66q4Nat9O6nJam21j6MjKz/P7snJxcHZss + kJOT7fdnZWb4jKl7RdRcUQt0lQ/dFU7z4nQ4jabBNJK5efn5GwIFOrZZILAhPz8v16ga + UiPqdFiv0dVB11rvzoQkczk9632ZfmMZKCgqLikNBsvKynVssEBZWTBYWlJcVBAwpv5M + 33qPuaJJCdZX0ccX1FxPw5noSEn1eDP8ufmBwuJg2caKyspQKPSFji0WMBSVlRUby4LF + hYH8XH+G15OaYm5oXOwqT1xzPc2nUPIKZ3ZeoKi0vKIyVLX5y3BNrTl1Ov/4ApZDTfjL + zVWhyory0qJAXvYKaLL5KFrlgq411zMx2enyeDOz8wpKyjeFqsM1ddvqGxqbmpqadWyw + gIFobKjfVlcTrg5tKi8pyMvO9HpczuREc0E/fOC+u54Ow5mRnV8YrAhVb6mrb2xu2dG6 + q639ax1bLNDetqt1R0tzY33dlupQRbAw37qhLsdqF9TyNE9bt+HMKwxWVoW3NjRt39m+ + e9/+joOdOjZZ4GDH/n2723dub2rYGq6qDBZaj1x3irmgHz1wzeM2PtHhSvP58wqCm6pr + 65tb2/Ye6Pyuq7vnaCRyTMcGC0QiR3u6u77rPLC3rbW5vrZ6U7Agz+9Lsy7oRw/ctevM + zyrmembmBkoqqmobWnbt6TjU1RM50ftDX/+pAR0bLHCqv++H3hORnq5DHXt2tTTUVlWU + BHIzzQU1P7N8+AK1Hrfm7enz5xeVh8L1LW37Og8fOd7bN3DmbPTcoI4tFjgXPXtmoK/3 + +JHDnfvaWurDofKifL/PeoOaB+5ff0W04pm6PiM3ULqpemuz4eyO9Pafjg4OXbg4fEnH + FgsMX7wwNBg93d8b6TagzVurN5UGcjPWp67qGZ/kdHuz8grLQ1saWvd0dh87ORAdvDA8 + cnn06piOLRa4Onp5ZPjCYHTg5LHuzj2tDVtC5YV5WV63Myn+o/sZG5+UYj1uiyuq65ra + Ow5HTv4YHRr+ZXTs+o2bv+rYYoGbN66Pjf4yPBT98WTkcEd7U111RbH1wE1JMh9EHzxv + YxOSXWkZOYHSyvBX2/ceOvL9QHTo0pWx8Ylbk7dv39GxwQK3b0/emhgfu3JpKDrw/ZFD + e7d/Fa4sDeRkpLmSE1bxdLjM67OgLFTTuPNA1/E+wzl67eatqbv3ph/M6NhggQfT9+5O + 3bp5bdSA9h3vOrCzsSZUVmBeoC7HKp7m89ab9VnRxirzuO3s6T09+POVaxOTd+7PzD6c + e6RjgwXmHs7O3L8zOXHtys+Dp3t7Os0Dt2pj0WdZXvOB+9H9ND+upHqt1+fmbS27v430 + Rc+PjN2cvDv9+9zj+SdPdWywwJP5x3O/T9+dvDk2cj7aF/l2d8u2zdYL1Gt94H74/jSe + buNZUhmu37Gv68TA4PDo+K0707OPnjxdWFzSscECiwtPnzyanb5za3x0eHDgRNe+HfXh + yhLj6V7V0+lO928wn0MNrfu7e8/8NDI2MXXfcC4sPVvWscUCz5YWDOj9qYmxkZ/O9Hbv + b20wH0Qb/Olu52r30+lJzw4EP69p3NXRc/Ls+cvXf7s7Mze/sLT8/MVLHRss8OL58tLC + /NzM3d+uXz5/9mRPx67Gms+Dgex0zyc8fTmBYKi2qe3g0b7oxdHxyXuzj/9YXH7+8pWO + LRZ4+Xx58Y/Hs/cmx0cvRvuOHmxrqg0FAzm+T3uaH1eM5zeR/nPDV29MTT+cX3hmOF+/ + 0bHBAq9fvXz+bGH+4fTUjavD5/oj31ieZQV/69neGTk1eGls4vaDuSeLyy8M51sdGyzw + 5vWrF8uLT+Ye3J4YuzR4KmJ+YPmUp/nrskSnx5ezcj9X8fyXzj++wFvs+Zf/x8namDjz + 61vz66HyL+qav+48NmDu5693Zh49XVp++erN23/8z6ICZoG3b169XF56+mjmzq/mfg4c + 6/y6ue6LcvMLIvML3LgYebL9SyJPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5 + sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypP + NjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJ + Job7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZ + xHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSeb + GO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQT + w33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxi + uK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M + 95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonh + vvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHc + V554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7 + yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBf + eeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4r + T7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33l + ifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK88 + 8T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un + 3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE + +7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554 + H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPv + w5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ9 + 2FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wP + WypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdh + S+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5s + qTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3oct + lSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl + 8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZU + nmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bK + k00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5 + sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypP + NjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJ + Job7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZ + xHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSeb + GO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQT + w33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxi + uK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M + 95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonh + vvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHc + V554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7 + yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBf + eeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4r + T7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33l + ifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK88 + 8T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un + 3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE + +7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554 + H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPv + w5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ9 + 2FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wP + WypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdh + S+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5s + qTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3oct + lSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl + 8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZU + nmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bK + k00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5 + sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypP + NjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJ + Job7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZ + xHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSeb + GO4rT7wPWypPNjHcV554H7ZUnmxiuK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQT + w33lifdhS+XJJob7yhPvw5bKk00M95Un3octlSebGO4rT7wPWypPNjHcV554H7ZUnmxi + uK888T5sqTzZxHBfeeJ92FJ5sonhvvLE+7Cl8mQTw33lifdhS/8bz3VxiU6PL6egLFTb + 1N4ZOTV4aWzi9oO5J4vLL169fvOW7Y/+f7Hv2zevX71YXnwy9+D2xNilwVORzvam2lBZ + QY7P40yMW7d2zZ/P33i+1fnnF8Cef9Zcs+bPnm3fRPrPDV+9MTX9cH7h2fOX5oLq2GCB + 169ePn+2MP9weurG1eFz/ZFv2v58P1f1DATN87bt4NG+6MXR8cl7s4//WFw2oDq2WODl + 8+XFPx7P3pscH70Y7Tt60PIMBt4/b1fxTM8OBD+vadzV0XPy7PnL13+7OzM3v7C0/PzF + Sx0bLPDi+fLSwvzczN3frl8+f/ZkT8euxprPg4Hs9JX358ee7nT/htLKcEPr/u7eMz+N + jE1M3Z999GRh6dmyji0WeLa08OTR7P2pibGRn870du9vbQhXlm7wp7ut76GPPB1urz+/ + pDJcv2Nf14mBweHR8Vt3pg3o04XFJR0bLLC48NRwTt+5NT46PDhwomvfjvpwZUm+3+t2 + rOqZajyLKzZva9n9baQven5k7Obk3enf5x7PP3mqY4MFnsw/nvt9+u7kzbGR89G+yLe7 + W7Ztrig2nqmreMYmOlzerM+KNlbVmR9Ae3pPD/585drE5J37M7MP5x7p2GCBuYezM/fv + TE5cu/Lz4OneHvPjZ13VxqLPsrwuR2Lsh8/b2ASHa31GrvmFQk3jzgNdx/uiQ5dGr928 + NXX33vSDGR0bLPBg+t7dqVs3r41eGor2He86sLOxxvw6ITdjvcuRsIpnsistIydgPoi+ + 2r730JHvBwzolbHxiVuTt2/f0bHBArdvT96aGB+7YjgHvj9yaO/2r8znUCAnI82VvIpn + fFKKx2e9QKvNA7fjcOTkj9Gh4V9Gx67fuPmrji0WuHnj+tjoL8ND0R9PRg53mMdttfX6 + 9HlSkuI/up8x8UlOtzcrr7A8tKWhdU9n97GTA9HBC8Mjl0evjunYYoGro5dHhi8MRgdO + Huvu3NPasCVUXpiX5XU7k+JjPnx/xsQlOlLNCzRQuql6a3Pbvs7uSG//6ejg0IWLw5d0 + bLHA8MULQ4PR0/29ke7OfW3NW6s3lQbM69P6vF3V02U9cIvKQ+H6FgN6+Mjx3r6BM2ej + 5wZ1bLHAuejZMwN9vcePHDacLfXhUHmR9bg1n7cfea5dF5eQlOL2ZuYGSiqqahtadu3p + ONTVEznR+0Nf/6kBHRsscKq/74feE5GerkMde3a1NNRWVZQEcjO97pSkhA//umzN2nWx + 8eYn0DSfP68guKm6tr65tW3vgc7vurp7jkYix3RssEAkcrSnu+u7zgN721qb62urNwUL + 8vy+NHM9zefQX//603iaF2iyuaAZ2XmFwcqq8NaGpu0723fv299xsFPHJgsc7Ni/b3f7 + zu1NDVvDVZXBwrzsDHM9k63H7Sqe1gX1GND8wmBFqHpLXX1jc8uO1l1t7V/r2GKB9rZd + rTtamhvr67ZUhyqChfmG03p7xn/saT1wzQV1GtDM7LyCkvJNoepwTd22+obGpqamZh0b + LGAgGhvqt9XVhKtDm8pLCvKyMw2n01zPjx63a1YeuPHmiZtq3dC8QFFpeUVlqGrzl+Ga + WnPqdP7xBSyHmvCXm6tClRXlpUUB62HrSTVP29Wup/E0FzQh0bEC6s/NDxQWB8s2VlRW + hkKhL3RssYChqKys2FgWLC4M5Of6VzgdiQnmen74+jR/GWq+iAxokiPF5Vnvy/Tn5uUH + CoqKS0qDwbKych0bLFBWFgyWlhQXFQTy83L9mb71HleKw/ysEvvR15D1d9vmgsbExZsb + 6nS507y+DH92jjHN3xAo0LHNAoEN+cYyJ9uf4fOmuV1Oczutp+0q1/M9qHnkJpsr6k5b + b0gzs/xGNSdXxyYL5BhJf1amwVxvNFMcyeZh+ynONWtXbmhcfOKKaKrHY0y96ek+X4aO + TRbw+dLTvcbS40ld0TSfQiucH/zw+f7/SrQCGhtnrqgRdThTXK5Ut9ujY6sF3O5UlyvF + 6TB307qc5t25bu3qnOaJa26o9VVkvUYTk5INqsPpdKbo2GYBw2FQkpOTDKa5m5bmpzmt + j6J3oobUmBrUlZOkY5MF3oMkWJZxsX+rufKZa4mui4mJibVQdWy5gKGMsa4mvJvv36LW + JV0xtf55c8z/Usc2C7wzWflPw/S/ZP/JfzH/vI5NF/hP/PTPaAEtoAW0gBbQAlpAC2gB + /gX+B05tLvoKZW5kc3RyZWFtCmVuZG9iagoxNyAwIG9iago2NTUwCmVuZG9iagoxOCAw + IG9iago8PCAvTGVuZ3RoIDE5IDAgUiAvVHlwZSAvWE9iamVjdCAvU3VidHlwZSAvSW1h + Z2UgL1dpZHRoIDE4NCAvSGVpZ2h0IDEwMCAvQ29sb3JTcGFjZQovRGV2aWNlR3JheSAv + SW50ZXJwb2xhdGUgdHJ1ZSAvQml0c1BlckNvbXBvbmVudCA4IC9GaWx0ZXIgL0ZsYXRl + RGVjb2RlID4+CnN0cmVhbQp4Ae2b+VeS6RvGrdxARBAERRQFXAAVUQrFUgwzUcxdcVwG + Qx1pMKqRkaPlkilFrpnjMu7jMuo4Wk46TXXmX/vezwuVC1Jzjt/HfuD6Jc/hxPvx6n5f + 6Lmvy8vLI48DHgc8Dvy/HbjwDeg//Y7Ae/GzLp2LPl//IuB8Bb6DGVi9vX3OXd7eAIJ+ + hS+hE9QOZF8/P3+HSNjlvLCfny9YB/BfIHdS+/j4AjKJTA4ICKBQKIHnILgsXJxMJvn7 + I3b35Aj7EkwHQAMyJZBKDaLR6OckGi2ISg0EejKgO8hPGRYCG7xG1IHUIDo9mMFkhoSw + WGzsYrFCQphMRjCdHkQNROTgOQyLa3DkNjIbUdOAmcUODeNwwrncCOzicsM5nLBQNgvY + aQQ5WI7AXTxYCGwY7AAKUAM0EEfwoqKi+YJzED86KooXAfSADuSUADTmrsEvoNn2I4HZ + dAYrlAPMfEFMbFy8SCQWS7BKLBaJ4uNiYwR8YOeEshh0sJzkh+7Ok4aD3YDtHxAYRGey + ORFRfGGsSJyQKJXKZLIUzIJLSqWJCWJRrJAfFcFhM+lBgeC4j7eLSQG74ZYkE9jhPH5M + vCRRKku9fEWRrgRlYBS6XrriyuVUmTRREh/D54UT4GS4OV0YfgHs9idTqHRmaDhPECdJ + kskV6RnXslTZarU6B6vggtmqrGsZ6Qq5LEkSJ+CFhzLpVArZHww/PigOuwMAmx0eJRQl + yuRpGVnZObl5mgJt4S3MKtQWaPJyc7KzMtLkskSRMAo5Tg1wZTjihimhATZPKJKmKq6q + 1DfzC4tKyyoqddhVWVFWWlSYf1OtuqpIlYqEaFRogWD4iUGBMfH1D6AGszg8gShJrszK + 0WhLynU1dfX62wZDI1YZDLf19XU1uvISrSYnSylPEgl4HFYwMvzEoFy4CM9AsDs0gh+X + mKpU5RYUV1TX6Q3Nxh9NrXfNWHW31fSjsdmgr6uuKC7IVSlTE+P4EaFgODwLjw84GhOY + bhYnKkYiU2Tlakt1tQ1NRpP5QZul3YpZ7Za2B2aTsamhVleqzc1SyCQxURwWmnAYlKMf + mQR3EIMdwY9Pkl/NAex6g7H1vsXa+airpxezeroedVot91uNhnoAz7kqT4rnR7AZQS65 + fUkUGjOMJ5TI0lSaYl194x2zxfqop6/f9syOWc9s/X09j6wW853Gel2xRpUmkwh5YUwa + heR7wm9vX1IgGpPYRHmGurCi1nDnnqWz54nNPjQy+gKzRkeG7LYnPZ2We3cMtRWF6gx5 + YiwalEAS3JjH5sTbj0wNZnP58VJF5s2S6oYWs6Wzd8A+PPZyYmpqGqumpiZejg3bB3o7 + LeaWhuqSm5kKaTyfyw6mkv1ccAdQYbwFYll6dn55XZMJsG2Doy8nZ2bnFxaxamF+dmby + 5eigDcBNTXXl+dnpMrEABpwa4IIbHifMsMiYhFQYE53eeN/aPTA4NjE9t7i8srqGVasr + y4tz0xNjgwPd1vtGvQ4GJTUhJjKMCQ+UE37DYzCIicb78rXcou8MJsvDPvvoxMz80ur6 + xuYWVm1urK8uzc9MjNr7HlpMhu+Kcq9dRgPORA+U4/MN3DTgjpMqsvJK65rN1h7b8Mvp + +eW1za3tnV2s2tne2lxbnp9+OWzrsZqb60rzshTSOOCmueSm0EI40XBbqjRl9cYHHX32 + sck5wN7efbWHWa92twF8bnLM3tfxwFhfplHBjRnNCaFRXPlNoYeE80XJ6dkFFfo7bQ/7 + h8ZnFlc3tnf33uwfYNX+m73d7Y3VxZnxof6HbXf0FQXZ6ckifngI/RRuFpcvkinV2srb + JkuXbXhidnn9j529NwdvMevgzd7OH+vLsxPDti6L6XalVq2Uifhc1unc8BgE7ipDa3vP + s5HJ+ZWN7VeA/e49Vr17e/Dm1fbGyvzkyLOe9lZDFeIWC77IXagz3LX22semFlY3d/b2 + AfsDVr1/93Z/b2dzdWFqzN5rvWuAB+Fp3PA11p9CZ3EJv11w/4tRH9xzH/mf2oVLPvD1 + BD4uJSkZObd0jWbw+8X04trW7t7B2/cfMFLDpT68f3uwt7u1tjj9Avw2N+pu5WSkSOAD + E76g+FzycJ/1v4bH77N21P37efx2789Zv+rx+6wddf9+Hr/d+3PWr3r8PmtH3b+fx2/3 + /pz1qx6/z9pR9+/n8du9P2f9qsfvs3bU/ft5/Hbvz1m/+h/89vrC+cm3eu5zOvfr/b// + wXrKBhf75+/91193XnWEmzgf7H76LZwPPu3+uvNBdB57XVvZ8Ok8duu8zmO3Pp3HNlRq + r7s/jz16/j04PrMAB7K7r//ax6y/XsP598rCzPjgV51/H9o3tDzoeGwfnZxdWtvY3sG/ + b9jZ3lhbmp0ctT/ueNDypX2DY7+TdCUrr6S22dzebRsah/3O6sbW9p87WPXn9tbGKux3 + xods3e3m5tqSvKwrSafvd5z7NLljn9YG+7QRtE9bWf99E7N+X19B+7QR2Ke1OfZpxMLY + 5T7NG/aXjFDH/lJbpTfes3b1Px+dmJpdWPptZRWrVn5bWpidmhh93t9lvWfUw7qB2F+G + wt715P7S2+/zvliD9sVtHb0Dz0fGf5n+dW5+Aavm536d/mV85PlAb0cb2hdr3O6LYT9P + d+7nc4ur9cR+vt8+NDo+MTmFWZMT46ND9n5iP6+vLs517ufpLvfzKA8R4sxDaCEPYYRg + QXefzT44PDKGWSPDg3ZbXzfECoyQh4AxIcY7xGUe4pIjfxIpFEP+JK+4CmIz5jZrZ/fj + JwNPMQdQnj0dePK4u9PaBtj1VcV5kD8RC2E97zJ/gnJKH/M+GeoClPdpMd1ra+942NWN + OfDT0931sKO97Z6pBeV9CsBuN3kf+EJI5Kt4znxVSVWNvrHFZL7/k+VnzAGr9p8tP903 + m1oa9TVVJc58FQTaiHzVxWO5AiLPBkEllGdLSFGqbuQXletq9YamH1CgrfUuRrWiONsP + TQZ9ra68KP+GSpmS4MizUVzk2Zz5QUgqRQogGZaeqc7TFpVVVtfUf48ChHh1W/99fU11 + ZVmRNk+dmQ5TIoh02O0qP4jymhQagw3Bx/ikFEWGSp2r0RaVlJZXVFbhDWxWVVaUl5YU + aTW5alWGIgWweRw2g4YCsifyml5EHNmRjwXwBEj1KjNV6hs3NfkFWm0hVmm1BfmamzfU + qkwlJHsTAPtQPvZoLMzLC+WRIbVOBHshjyxOlKVeSVNey1Spsq9DQBirrmerVJnXlGlX + UmWJYpRHJmK9KLl+PK6JuIn8NwpSszmR0cI4cYJUliK/rFCkEQlwlMrGpfQ0heKyPEUm + TRDHCaMjUXAdxahPyX878vYUR96eFy2MjRdJEpKkyckQf8es5GRpUoJEFB8rjIamAIqt + o4aDy7w9MpwoCoDjqN/AjYyKFghjiYIDNBxwSkTUG2KFguioSK6j3+DAPnlXwrR/qsFA + n4QOfZIwDjcikiiUYG+U8Ik6SWQElxMGfRI69EnIqHvkuk/yCZyEmjB0BjOE7SjwQIMH + t8Id9R12CJOBqANIbrC9nDUvoi9FgYoXnc5wFqZY2EXUpRh0OpS9KI6+FKqoue5LfQRH + TS+So592fgU1op5G9NOgWuds1p2CDSMOM04064g+IAn1AYlGILTzcIu4NJn0VX1AdHMS + 5KgU6OP7uYDprEPi/gPql0T/kqiOnmq28wPUWdJFfddzL7wCwse665FU1fHP+s/oyPbD + wtkxPnxd+BmM/Brow78L+ivnqcMsnp89Dngc8DjgceCoA/8DzflMvgplbmRzdHJlYW0K + ZW5kb2JqCjE5IDAgb2JqCjI4MjgKZW5kb2JqCjIwIDAgb2JqCjw8IC9MZW5ndGggMjEg + MCBSIC9OIDMgL0FsdGVybmF0ZSAvRGV2aWNlUkdCIC9GaWx0ZXIgL0ZsYXRlRGVjb2Rl + ID4+CnN0cmVhbQp4AYVUz2sTQRT+Nm6p0CIIWmsOsniQIklZq2hF1Db9EWJrDNsftkWQ + ZDNJ1m426+4mtaWI5OLRKt5F7aEH/4AeevBkL0qFWkUo3qsoYqEXLfHNbky2perAzn7z + 3jfvfW923wANctI09YAE5A3HUqIRaWx8Qmr8iACOoglBNCVV2+xOJAZBg3P5e+fYeg+B + W1bDe/t3snetmtK2mgeE/UDgR5rZKrDvF3EKWRICiDzfoSnHdAjf49jy7I85Tnl4wbUP + Kz3EWSJ8QDUtzn9NuFPNJdNAg0g4lPVxUj6c14uU1x0HaW5mxsgQvU+QprvM7qtioZxO + 9g6QvZ30fk6z3j7CIcILGa0/RriNnvWM1T/iYeGk5sSGPRwYNfT4YBW3Gqn4NcIUXxBN + J6JUcdkuDfGYrv1W8kqCcJA4ymRhgHNaSE/XTG74uocFfSbXE6/id1ZR4XmPE2fe1N3v + RdoCrzAOHQwaDJoNSFAQRQRhmLBQQIY8GjE0snI/I6sGG5N7MnUkart0YkSxQXs23D23 + UaTdPP4oInGUQ7UIkvxB/iqvyU/lefnLXLDYVveUrZuauvLgO8XlmbkaHtfTyONzTV58 + ldR2k1dHlqx5erya7Bo/7FeXMeaCNY/Ec7D78S1flcyXKYwUxeNV8+pLhHVaMTffn2x/ + Oz3iLs8utdZzrYmLN1abl2f9akj77qq8k+ZV+U9e9fH8Z83EY+IpMSZ2iuchiZfFLvGS + 2EurC+JgbccInZWGKdJtkfok1WBgmrz1L10/W3i9Rn8M9VGUGczSVIn3f8IqZDSduQ5v + +o/bx/wX5PeK558oAi9s4MiZum1Tce8QoWWlbnOuAhe/0X3wtm5ro344/ARYPKsWrVI1 + nyC8ARx2h3oe6CmY05aWzTlShyyfk7rpymJSzFDbQ1JS1yXXZUsWs5lVYul22JnTHW4c + oTlC98SnSmWT+q/xEbD9sFL5+axS2X5OGtaBl/pvwLz9RQplbmRzdHJlYW0KZW5kb2Jq + CjIxIDAgb2JqCjczNwplbmRvYmoKOCAwIG9iagpbIC9JQ0NCYXNlZCAyMCAwIFIgXQpl + bmRvYmoKMjIgMCBvYmoKPDwgL0xlbmd0aCAyMyAwIFIgL04gMSAvQWx0ZXJuYXRlIC9E + ZXZpY2VHcmF5IC9GaWx0ZXIgL0ZsYXRlRGVjb2RlID4+CnN0cmVhbQp4AYVST0gUURz+ + zTYShIhBhXiIdwoJlSmsrKDadnVZlW1bldKiGGffuqOzM9Ob2TXFkwRdojx1D6JjdOzQ + oZuXosCsS9cgqSAIPHXo+83s6iiEb3k73/v9/X7fe0RtnabvOylBVHNDlSulp25OTYuD + HylFHdROWKYV+OlicYyx67mSv7vX1mfS2LLex7V2+/Y9tZVlYCHqLba3EPohkWYAH5mf + KGWAs8Adlq/YPgE8WA6sGvAjogMPmrkw09GcdKWyLZFT5qIoKq9iO0mu+/m5xr6LtYmD + /lyPZtaOvbPqqtFM1LT3RKG8D65EGc9fVPZsNRSnDeOcSEMaKfKu1d8rTMcRkSsQSgZS + NWS5n2pOnXXgdRi7XbqT4/j2EKU+yWCoibXpspkdhX0AdirL7BDwBejxsmIP54F7Yf9b + UcOTwCdhP2SHedatH/YXrlPge4Q9NeDOFK7F8dqKH14tAUP3VCNojHNNxNPXOXOkiO8x + 1BmY90Y5pgsxd5aqEzeAO2EfWapmCrFd+67qJe57AnfT4zvRmzkLXKAcSXKxFdkU0DwJ + WBR9i7BJDjw+zh5V4HeomMAcuYnczSj3HtURG2ejUoFWeo1Xxk/jufHF+GVsGM+Afqx2 + 13t8/+njFXXXtj48+Y163DmuvZ0bVWFWcWUL3f/HMoSP2Sc5psHToVlYa9h25A+azEyw + DCjEfwU+l/qSE1Xc1e7tuEUSzFA+LGwluktUbinU6j2DSqwcK9gAdnCSxCxaHLhTa7o5 + eHfYInpt+U1XsuuG/vr2evva8h5tyqgpKBPNs0RmlLFbo+TdeNv9ZpERnzg6vue9ilrJ + /klFED+FOVoq8hRV9FZQ1sRvZw5+G7Z+XD+l5/VB/TwJPa2f0a/ooxG+DHRJz8JzUR+j + SfCwaSHiEqCKgzPUTlRjjQPiKfHytFtkkf0PQBn9ZgplbmRzdHJlYW0KZW5kb2JqCjIz + IDAgb2JqCjcwNAplbmRvYmoKMTMgMCBvYmoKWyAvSUNDQmFzZWQgMjIgMCBSIF0KZW5k + b2JqCjI0IDAgb2JqCjw8IC9MZW5ndGggMjUgMCBSIC9OIDMgL0FsdGVybmF0ZSAvRGV2 + aWNlUkdCIC9GaWx0ZXIgL0ZsYXRlRGVjb2RlID4+CnN0cmVhbQp4AdWWd1gTSR/HZ3fT + Cy0QWoDQe+8gvQYQkCqISkgooYQQioDYkMMTPAsqImABPRBR8CyAnAURxcIhYK8X5BBQ + z8OCqKi5Dbx4/vHef+8/7+yzM5/9zm9/Mzsz+zxfACg9bIEgHZYBIIOfIwzz82Qujoll + 4h8AHGAAMpAFMmxOtsAjNDQI/Gt5fwdAks6bppJc/xr23ztkuYnZHACgULQ7gZvNyUD5 + JMpfOQJhDgCwhAdW5AhQRkpQlheiE0S5SsLJc3xEwglz3DUbExHmhcbcAoBAYbOFyQCQ + RajOzOMko3koKAILPpfHR9kCZVdOCpuLsgBlk4yMTAnXomyQ8F2e5O+YzU74lpPNTv7G + c9+CvokO7M3LFqSzC2Yf/pdVRnouul6zRQ6tKfz04CC0paP3OJftHTjPgvTZPZvVE/mR + 4fM6PyE4ZJ6ThL5h8yzI8fyOQyPm9cIUr+B5Tsz2+ZYnlR0g2bPZ/MLcsMh5zs4L95nn + wpSI6HnmJnp/05N4vqx5nZfD+jZWWmbgtzkAb+ADgtCLCayAxezlC9ARcxLz0T0EwCtT + UCDkJafkMD3QU5dowmTxOWYmTCsLS8mW/v8Uyf82N9u3N2b/I0hJcpT/o+UvAcBdGT3L + 0/9oUej53zsEgOKtfzQDTQA0FQBoT+DkCvPm8mEkDRaQgDSQB8pAHWgDA2CKrqYdcAbu + 6OoGgBAQAWLAMsABKSADCMEKUATWgVJQDraCnaAa7AMHwCFwFBwH7eAMuAAug+tgANwG + D4EIjIIXYBK8BzMQBOEhKkSDlCENSBcyhqwgB8gV8oGCoDAoBoqHkiE+lAsVQeuhcqgC + qobqoCboF+g0dAG6Cg1C96FhaAJ6A32CEZgCy8NqsB5sDjvAHnAgHAEvhZPhLLgQLoE3 + w1VwPXwEboMvwNfh27AIfgFPIQAhI3REEzFFHBAvJASJRZIQIbIaKUMqkXqkBelEepGb + iAh5iXzE4DA0DBNjinHG+GMiMRxMFmY1ZhOmGnMI04bpwdzEDGMmMV+xVCwDa4x1wrKw + i7HJ2BXYUmwltgF7CnsJexs7in2Pw+HoOH2cPc4fF4NLxa3EbcLtwbXiunCDuBHcFB6P + V8Yb413wIXg2Pgdfit+NP4I/jx/Cj+I/EMgEDYIVwZcQS+ATigmVhMOEc4QhwhhhhihD + 1CU6EUOIXGIBcQvxILGTeIM4SpwhyZL0SS6kCFIqaR2pitRCukR6RHpLJpO1yI7kRWQe + eS25inyMfIU8TP5IkaMYUbwocZRcymZKI6WLcp/ylkql6lHdqbHUHOpmahP1IvUJ9YMU + TcpMiiXFlVojVSPVJjUk9UqaKK0r7SG9TLpQulL6hPQN6ZcyRBk9GS8ZtsxqmRqZ0zJ3 + ZaZkabKWsiGyGbKbZA/LXpUdl8PL6cn5yHHlSuQOyF2UG6EhNG2aF41DW087SLtEG5XH + yevLs+RT5cvlj8r3y08qyCnYKEQp5CvUKJxVENERuh6dRU+nb6Efp9+hf1JUU/RQTFTc + qNiiOKQ4raSq5K6UqFSm1Kp0W+mTMlPZRzlNeZtyu/JjFYyKkcoilRUqe1UuqbxUlVd1 + VuWolqkeV33AgBlGjDDGSsYBRh9jSk1dzU9NoLZb7aLaS3W6urt6qvoO9XPqExo0DVcN + nsYOjfMaz5kKTA9mOrOK2cOc1GRo+mvmatZp9mvOaOlrRWoVa7VqPdYmaTtoJ2nv0O7W + ntTR0FmoU6TTrPNAl6jroJuiu0u3V3daT18vWm+DXrveuL6SPku/UL9Z/5EB1cDNIMug + 3uCWIc7QwTDNcI/hgBFsZGuUYlRjdMMYNrYz5hnvMR40wZo4mvBN6k3umlJMPUzzTJtN + h83oZkFmxWbtZq/MdcxjzbeZ95p/tbC1SLc4aPHQUs4ywLLYstPyjZWRFceqxuqWNdXa + 13qNdYf1axtjm0SbvTb3bGm2C2032HbbfrGztxPatdhN2OvYx9vX2t91kHcIddjkcMUR + 6+jpuMbxjONHJzunHKfjTn85mzqnOR92Hl+gvyBxwcEFIy5aLmyXOheRK9M13nW/q8hN + 043tVu/21F3bneve4D7mYeiR6nHE45WnhafQ85TntJeT1yqvLm/E28+7zLvfR84n0qfa + 54mvlm+yb7PvpJ+t30q/Ln+sf6D/Nv+7LDUWh9XEmgywD1gV0BNICQwPrA58GmQUJAzq + XAgvDFi4feGjYN1gfnB7CAhhhWwPeRyqH5oV+usi3KLQRTWLnoVZhhWF9YbTwpeHHw5/ + H+EZsSXiYaRBZG5kd5R0VFxUU9R0tHd0RbRosfniVYuvx6jE8GI6YvGxUbENsVNLfJbs + XDIaZxtXGndnqf7S/KVXl6ksS192drn0cvbyE/HY+Oj4w/Gf2SHsevZUAiuhNmGS48XZ + xXnBdefu4E4kuiRWJI4luSRVJI0nuyRvT55IcUupTHnJ8+JV816n+qfuS51OC0lrTBOn + R6e3ZhAy4jNO8+X4afyeTPXM/MxBgbGgVCDKcsramTUpDBQ2ZEPZS7M7cuRRY9OXa5D7 + Q+5wnmteTd6HFVErTuTL5vPz+wqMCjYWjBX6Fv68ErOSs7K7SLNoXdHwKo9Vdauh1Qmr + u9dorylZM7rWb+2hdaR1aet+K7Yorih+tz56fWeJWsnakpEf/H5oLpUqFZbe3eC8Yd+P + mB95P/ZvtN64e+PXMm7ZtXKL8sryz5s4m679ZPlT1U/izUmb+7fYbdm7FbeVv/XONrdt + hypkKworRrYv3N62g7mjbMe7nct3Xq20qdy3i7Qrd5eoKqiqY7fO7q27P1enVN+u8axp + rWXUbqyd3sPdM7TXfW/LPrV95fs+7eftv1fnV9dWr1dfeQB3IO/As4NRB3t/dvi5qUGl + obzhSyO/UXQo7FBPk31T02HG4S3NcHNu88SRuCMDR72PdrSYttS10lvLj4Fjucee/xL/ + y53jgce7TzicaDmpe7L2FO1UWRvUVtA22Z7SLuqI6Rg8HXC6u9O589SvZr82ntE8U3NW + 4eyWc6RzJefE5wvPT3UJul5eSL4w0r28++HFxRdv9Szq6b8UeOnKZd/LF3s9es9fcbly + 5qrT1dPXHK61X7e73tZn23fqN9vfTvXb9bfdsL/RMeA40Dm4YPDckNvQhZveNy/fYt26 + fjv49uCdyDv37sbdFd3j3hu/n37/9YO8BzMP1z7CPip7LPO48gnjSf3vhr+3iuxEZ4e9 + h/uehj99OMIZefFH9h+fR0ueUZ9VjmmMNY1bjZ+Z8J0YeL7k+egLwYuZl6V/yv5Z+8rg + 1cm/3P/qm1w8Ofpa+Fr8ZtNb5beN72zedU+FTj15n/F+Zrrsg/KHQx8dPvZ+iv40NrPi + M/5z1RfDL51fA78+EmeIxQK2kD3rBRC0hpOSAHjTCAA1BgDaAACkrjk/PBuBOvhh/3n6 + N57zzLNRdgDUuQMgsXdBXQDsQls99JmBthJbiFo6eO0/N6pISnaStdUsQBQV1Jp0icVv + xADg4wH40i8Wz1SJxV8qUa/zDoDzwXM+XBItg/r7/YiFjW9QZ6bb7OvfV38DJafpNwpl + bmRzdHJlYW0KZW5kb2JqCjI1IDAgb2JqCjI2NjMKZW5kb2JqCjE1IDAgb2JqClsgL0lD + Q0Jhc2VkIDI0IDAgUiBdCmVuZG9iago0IDAgb2JqCjw8IC9UeXBlIC9QYWdlcyAvTWVk + aWFCb3ggWzAgMCA2MTIgNzkyXSAvQ291bnQgMSAvS2lkcyBbIDMgMCBSIF0gPj4KZW5k + b2JqCjI2IDAgb2JqCjw8IC9UeXBlIC9DYXRhbG9nIC9PdXRsaW5lcyAyIDAgUiAvUGFn + ZXMgNCAwIFIgL1ZlcnNpb24gLzEuNCA+PgplbmRvYmoKMiAwIG9iago8PCAvTGFzdCAy + NyAwIFIgL0ZpcnN0IDI4IDAgUiA+PgplbmRvYmoKMjggMCBvYmoKPDwgL1BhcmVudCAy + OSAwIFIgL0NvdW50IDAgL0Rlc3QgWyAzIDAgUiAvWFlaIDAgNzgyLjg5IDAgXSAvVGl0 + bGUgKENhbnZhcyAxKQo+PgplbmRvYmoKMjkgMCBvYmoKPDwgPj4KZW5kb2JqCjI3IDAg + b2JqCjw8IC9QYXJlbnQgMjkgMCBSIC9Db3VudCAwIC9EZXN0IFsgMyAwIFIgL1hZWiAw + IDc4Mi44OSAwIF0gL1RpdGxlIChDYW52YXMgMSkKPj4KZW5kb2JqCjMwIDAgb2JqCjw8 + IC9MZW5ndGggMzEgMCBSIC9MZW5ndGgxIDkxNDQgL0ZpbHRlciAvRmxhdGVEZWNvZGUg + Pj4Kc3RyZWFtCngBvVp5eFRFtj91917S6e70vqS701s6+0ICbULShE5IgIRAEBMkmAQS + EiQaMcbBESYqjBIVRQSi+DkuGAKM0oQIDQw8hg9F33PGZRSXcRZHdHzzzOe8eTijI+l+ + 594OEfKNfvzhN31TVedU1a069Tunzql7b3rX3tYOSdAPNNQvbe3pAOmX1gtAwiu6W3sS + vPYClnNX9PU6EzybDkCv6ehZ1Z3ghUcB5PZVa9ZN3J+yEkA13NneiqX0u4h5cSdWJFgy + DUtPZ3fvjxK8dhTL0JqbV0y0p4SQT+tu/dHE/PAh8s6bWrvbscRfWj1mnp6bb+2VWEhT + YhnuWds+0Z80onyvA8FaNdwMMrgReKCQVkMzAP+Z3A4Mtort+FuZpdh2Q3Lpl6ARJP6G + 2oek8mXXL8591X7Rr9gqfI0Vskv9xZILxAIASoLtY4qtky3SfZipo9CQGYUaTOWYijBl + Zs4yQT8ZgocxPYWJhi5yP6zDtBnTY5iYSWovckfJ/SOMEDpG1oGFzA0pGMdindlhkisc + b0UJN/qk433Tx8eJGbX3ETGPJIFslpw8RX4GK8FBngMvuQOqIZ08fiiwxtGCTXuhB1M/ + JlrKCdk7klrgOEmywMsQvMcHqQw57Phzfrbjk/woRUYcp/1RBotfpiIXSnacsj/p+A/7 + KsdJTPsTTfsC2OOwY699jWNbapQ8PuJ4xB4leM/WRHGbHW897OgO7HCszJfa5++IUvtH + HEFsXxJSOIpnuBxF9vOOXH9UIMhn2+c7MvJ/5fDgjdjNiYN6QxqHzb7NcQ02pdor/ddg + Ok72kV2QQXaNeOc6jiGJyz1UE5ixI0p+fKg6Pd8bJXeEiqvTdwSq/d7AfIc3UOX3I73k + FX4jfz0/iy/gM/l03se7eCuvE7SCWlAJSkEuCAIfJT8fKXdwx8l+KEdY9h8SOIGNkhew + kjlOnpcqnz8iMAIlgKCLxv+IxktAFyX7R9UihcRhTqK4KHn+UKLq+ZCDESlGalBTIo0Z + 5kARgYK5ECEPRjnYZOgrN5VryzTBqvB3ZS1Sy6U887t/JmKP7JjX0BjZZ2+KFIhE3N50 + qbvpEvGdZe9t2NRekZk5b9G6Q309qzsq292VLe7Kdkwtkfv7Ok2R/jan8+DqHrHBGaF9 + LW0rOsWytT3S424PR1a7w86DfdJ9U5o7xOY+d/ggdFQubjzYEWoPj/SF+irdreGmQ20V + a5uvmGvz5FxrK/7FXBXiYGvFudqk+6bM1Sw2t4lzNYtzNYtztYXapLnExVd2NVTc2ovW + 6azsmueMpDdEahYubYw4W5vCUTKEleHbgD0FavYEpLP9YGFywQEQfx/TB2IZuzb+KXsW + 1LHu+P/SJajUo2KiYuWlcAoehF1wADgYRjodlsMgvEpW495eBqNwjqRCDvpeBqIwH14j + 8fib0AG7sX8vnIbtcBCUeE836LF1C/HG70A+hHQbbIw/Ax6YAT+FExDEUbfAWHxv/BC2 + LoJrYR/sx/v/i7ipg0xK/IX4eRBgIY65EVvejM+PHwAtZEEF1GPtRjhJvPQH8U4wQQlK + 9wT8DJ6GX8Ln5G4yGu+M98XfiH+EpmoCGzTgtZ6Mko/oA8xP40/E/xKPIRLpkIGztsA2 + eBbHP4DXKXStleRG0ku2ke1UiLqbGmU2scbYOOIQgDl4VaNXvg8ROApn4G/wNfmCMtFq + upd+KV4U/z9QwDxcpbiSdujD6168tuCajhOO5JHZpJ6sJ4+S7eQ3VAZ1LdVI3U79iPqU + rqOX0evo3zC3MiPsA+wgp4h9GT8ePxt/B4xgh+thLWzA1Z2GN+AC/JPQOJaNeEkJqSDL + 8eonu6ij5GlylKonp8gb1D7yB/Ix+YJ8Q7GUktJTmVQvtY3aT52mfk130dvpx+g/0F8y + ZSzFPs1+wnn538baYptjv46XxD+Kf4UuVgAXaqYC6uAGaMXV9sA0+Amu4nm8DqDWzsBL + 8Kp0fUxsMAZfIQpAtMRCCkgtXnVkAekgXeRJcgyvk5Isf6dQEZSM0lBGykY1UG1UN9VP + vUP101Y6g55LL6UP4PUKfY7+hv6GYZkURs/MYWrgAaabeRyvIWaYGWFeZ4NsGVvHLmH7 + 2c3sA/QK9k32HLeB28KNcF9wf0W3OJ+/mX8AtfMq2uwv0Za//THEg9IXwE2wgoRJG+xA + bTxNWmEArWsluQ/x6oH0eDO9gZ5D5aE1nIQfo7U+DuthM70Mno6/R++Dd9FS1uCQ/bCH + qQA7uxO1czfkoRVNXKFARiDd7/N63GkuJ7p8m9ViNhkNel2KVqNOUirkMoHnWIamCGRV + uqtanBFfS4Txuaurs0Xe3YoVrZdVtOBWdkaqruwTcYr3tWLTFT1D2LNjSs9QomdosidR + O0uhNDvLWel2Rn4VdjujZOnCRqQfDLubnJExia6V6IclOglplwtvcFaaOsPOCGlxVkaq + +joHKlvC2VnkaAjhkGdniY4jBApx4AjMbl2PDhZmiz0qIxZ3uDJidiONbbS3snVlpH5h + Y2XY6nI1YR1WLWrEObKzuiIoJ9yvXOleeX80BG0tItW6rDFCtzZFqBZxLE1mxOgOR4x3 + fGL6lr1EVT5wWWOE8la1tg9URUIt9yO4Itsicq0PIDevwYnDUpuaGiNk04QQooyrUVJR + 3ERM8LasdkZk7gp358DqFgQXFjWOWEIWyflGoL5xxBwyS0x21lHThhIXrv5o9qzsWWJZ + 4jJtSJR/vidR/9YpsTRtOPNHLOctmgSAiAi4a1DOiHOFNIkbhZ0hZu0zYGDFDMQJf00E + l9mF8syOUGgztDfCemtaI/0Nl8ToDCeEa1kdHpGZLVIQqmjC/i0D6mtQU9hf7XYOfInR + usU99vmVNa0TNZxX/SWIjaKiJ20lQlov0X1isPTiqjtN7k5Rv32STpF3myovq0BehEaU + OaLDAF7f6Io4m7ACT5NZ86Igq288SMiWpiiJb4pC2H4Uz6j0DcuxOUs0ta4wzo9MdhZW + ZLiQyslyVuHMVaKtOAecAzUrB5xVzk40JsYrldjQPtCUiwg2NCJOsBhnDDVZJ8n2pqZr + cJxccRy8BbsPNOEIqydGwFKqyh3HTnlZGExpX33jwsZIf9gaCYWbUAtovqfqGyOn0HKb + mrBX/qSkKPH6LtOEzAUoc34GthcmRsGzSz8O0TQwII7Z0Oh2RU4NDFgHxP2W4KMEplaE + JiqiIHYRIY+S/nq8Fwu3yyrpwOV2oVhNIqbT0KQvWRSe2b8f4eJJufHO6ShtsYTwjB8I + 4eDVIHzNVSFcMinpFQiXoswlIsIz/30Il12BcPn3IxyalBuFnIXShiSEK34ghGdfDcLh + q0K4clLSKxCuQpkrRYTn/PsQrr4C4ZrvR3jupNwo5DyUdq6E8PwfCOHaq0G47qoQXjAp + 6RUI16PMC0SEF/77EF50BcIN34/w4km5UchrUdrFEsJLfiCEr7sahBuvCuGmSUmvQHgp + ytwkInz9JMIhawQu98P9U9wu/OCOedllkONJidVCBXMrLMLUhw/VJVhWU0GwYTmTnIWN + mDZz+2Aj8mIqofbBZuxXgX2MWOpxiEvvfpT4RFKKfCuYpQdyGmkGWKzF10SYAM/xMpDj + E4n4U+K7FpVEiVnyBDUNYqST/IUapYEexrP3V0wv8wZbzW7hbuJO8ouxF4XPAYB1J/C5 + gIfyxLshIRcDOCYB3xXBG5hEHmn6wygwmMR3SPyHcEySY0nmMRyFhSWZefmFGpfGj6mC + 2RK9+Cf2xD9nR5nab/A9A46+KP6hdOJPxme5UvhdaEZGHpGrFValzV9Yre6SrVbzQUGr + lNHWAt4js6uV9pJMKidQcqSEKinI8GrVPCvY/GlGW5QMhNxGu4P323MUlL1IUcqXltp0 + fCBj2GMpswZsc5P9M8wzy35BduKDzlGyA/B9Qt2F2rELY3Xqv9eeHz+jDeZCefmYeI1p + gxqtMdis0QZzxnLGCJYaYzA/b/a6UHrxdH0aELOXFCe7wJRqdYHBqXMRVxpMp1xgsRtd + RO/CDN+fZRJ1qfi8ftddd0EzafYYCgumF88kKpJMOJ7Tk+LpxUXTfO40nuPdZaSwAB8Z + NDrshFOoiDvN7/OLha9oWvH0FKJaW3dD0w5XZ0F3W34DGS3TK++548ESl3yY/cezJ/pu + M3qVqZqMLF9zhkE2/dd3bj9xbOfA60uzaoa26m2cKsmWu4qsEbJM2csa5mc0vLyrunpw + fKctjaY3KbkKd6h69Yv3bd+dQs6Luu+L/57xsqdBA6nQE8oZ4vfY3rXRaUJyKsUCGO0s + r5Gn2hUKnV+wOC056hwSAI3Z4bzXdaJZArW0dvz8eRHVMUA08U8T1CTQM2kNnNzA6XxE + K8dMzxt9JEWW6kOwiIgTNKcUakQotBodJSGgd3sSIHF6ndFQ2HegZHfLK1///YM7FhcE + h6iOrVsf/PFR35zT7Onx/6ldGBuLXYjFIiXu2s3rPzu59/eH39y5/KBkz/iUS7/B1IEF + rLAnlLvHTAZNw8I+Ez1X0OzS0bSOs1v4JLtOYeWtVqParyW0n9JY7HK/0WzDV3/8Idfa + 9RMWgysrrR0LBtFKJiwGCfWYZB7TwCx4lXq5D1QpalylJlnNm5FjgXYRQjG0wpDkg2Qt + ZjIT5yMM4Vy4cslURGNRl0omI9kLGIzuHDQANJWEVRSK5kAVqaGQp859bDygXrvh53Pz + 7nuk5x7zgdS/Hn/rn0T7to2pi7y74p7h7qee/nDz7e+8RAo/xUf0a3B+qI5/wFjw6dqG + b2K8RBlat1N4zLLHQbMqKpnV6VXaZL0upAzphICFzFMcps+Sl+mz1veE92XnHO+5PzN+ + 5lac1ZzVUssE1uVJftxg9wQ5nje47DZebjcovPxO2x7bEbQVxmtI9tpYs1zJa1T+ZLuf + tfg9ObzfbPb533YNJYwEbUQykbfHg9ogbrcgFrnNiR2IVOl4qXoMayVUq8DNsDS+uiAs + wzl8GrVWnaLWqRlO6U2zenzgBLuPpNplRt4HCr3KR5JUbosLq1jMBBPin6TGTNySiT0p + gZyRmXEXuaUZbmluRqjx0rtS0fSmF09HoHFPcu400CDYxOfHTcrxhBo9N6NYq774Bfvw + zgcX5+kO8gvyF62bteiV2F+I6U/EoUif+/ydwyxxM3NuvHbhmrnPPPtSc/Gckq059TY1 + ceP7G4pUxHy3Vd19aICIHwjQg9twwxnZt/CNT20ok7dzcjtNknVBQxKnlZuNRosqSRMw + anltssqholQXdWaT+aJr1YaEJY43B89IQNVVtoc/Rehw45WWj72NpqidXlxYYDDqRfvh + 9IV6twavosKiF93loxqP0WZWLHKOjI5s385WTFtGUbspcu0LWy6upJ/YMiz55ZmxEvoz + tBUHZOMbvyOh2mJdjVAjaxSaZPcp91qH7Xv9Q5lHrYqQQBvSAqoz8jR0vQwXsJvlWrs8 + OYfPyWFtdI4hJzvAWvKUKn9Smc9vM+fm3etaWzHpeYOiBYyf/xL1fMn3lo9Jak/oPcud + bklVaDxetc+d6vNBugUzjULlgmSVMslrT/MRvzWA+0mpdUnaRYebcLkJnwuo1aJCjY7n + XGk+fyGqWFSv5FU9omZBcr7S7kJXTKg7lxcWDZX2xF59/nPVkST/zHteD/no4sH1L8S+ + IfwxEt79k5NV3m13nl6QFXuTqShzz773YsFrfR/seq7aX/rIkt8tqv8HsZMkkhN7+tTI + DY+/eOLAio1UtuR/NqKySybiaXHIxn/CoPI5Wi5DHaNdBHgaHYdsn6stAU5p7Znx0jOT + ai2vRZWiZ3RrUJMbj+CPyfjmHHviNcmGNuPYM6WxAyE8K9ByFgfFMYE2M+xlQ46XXhow + Mdjm0VExGEtjoHycl5kDPtgUKuEFXsUlGwWjypjsF/y4havNSxSrFEq3V26xu81yijF6 + XXajPYnjgbPavHSKPB3n1ATwowIZsQTEbykhOZAcLyrH7E+PkqRD3y5t/Lz6wtiF8Qlh + jKWi3Y7hXjcGxUArbnkxDOgxOooh0XgpMro1Yqzk9IjCNGlDIrVxJDSt6Zb+uixP6TPt + 79VlHL+xdvVjRyyBno49o0zu4ALPzHJP1ZKGJxZvGZ9OfXZj/Zah8a3U8e6CeU++Pv6K + GOtw3fQY2rkZI8PyUP4R7ixHMZyO8+v6uF6e1SkpnUmNEQ84k0Ju4S0WUAZkFhvJMQXM + YLbisYO7YmWSS0tYM65rTBMMksSCiLiky5YirgB9jIrgKsjG/fP3dZ6vzzpiz9sQCsyd + kW0dJXtQ/uWLfnbdM+MLqWfbSlcmGSqKbukafx2FRV9eEn+fcWE8U+J7bjM8HCocFHao + HzM8xwwLQ+q9hqjwivAu84nqv3XKawTObuKVdq3CzJvNesqfbLHK/HqzxRolMoxqE95Y + OgV9G9Ekt5uFx0+fIkWGnlND+QhvRIpNQkquU/qAqDETDBjEaBVmkm8Vs0wMXh5t0YSO + MHJp0YtSeD5KBK4/bsqbf+y5HTuexY8JF2P/+F3sItH+meslyUM7lj96cWT/efqD2OcY + xsdjL5DMi3hYCrG4VdDGqUfQPjWwIOTz076k6fQchlEJakol08iUfkFUkUYuWFJIjjqg + AbM2JUoqUTUJVymeSOrUoousLT8zfkaMNqKtTViaqJZJX6lxb96v330ja7Krrer7HkEz + Olq8i6JP0tSBteODos1UxN+lDzPz0C/mkpzQQzNkg+wO7WO6Qf1gBpfu8fqLXVWuOZ45 + /iWe6/wdnlW+dcp1SetUfe5eT6+31zeUOpyVQmOYYrOZnBSw6K1Gm0mfrctJT1Z0CT5v + sZfypiXJmcwU08s2ewrP2HMez1Tk8jKVmuIh15VrcZgMJr+xLN3H+9Mt+SqHX10G/hxz + Xv7IZGzF7ZXwrUE1UuJyg7mYozmKAVY84Yrb7RZJy/NJNuXTey0+l8rhAhl+YiR0Fp6R + 2Qyk7Fqss+pMLuJMTnOBK02VJPjlLuLzyuQkm3Hhd2XMUjU2FzEbMJNCrOSEpUyyiktG + gUfgFMkF44HO78sVwyoebcU9zrsTIVY84jmIGIl1HB58/eQLwRseXjk403/rQ5tn9f72 + 6N9unE3tY31lj3V0VabX3X66ouv9339xlidHSP3SvOuuu77Sg6eStIyauwZ/sWVp58yC + OXWhqgxzij03q/LRh954/ynqa7QlY/wLSsYuxZ2z6MWkHPkpFYmS8pCXMQSNNKeSayzo + yvBrSwD0Kn0y7aAp+qLBbLZg3J04AU6Ju7mJwDumHj8vOVYx2opn1UvneF+RGHqHD+/f + 79PnJ6XqHLP9G5Zu3coujb2zbbxyRoqCUFtkwl2rqJe2SbFCH6uRYq+4s/8zdNOA/j7T + HhPNc0ZuhrZa26hdxd9O384/oBuEneygfqdhp3EYhg3qapinn2N8Vc+E2ZdZ6l52CIbI + HnbYyHrSWZPeaMB4o1cqku2CSnQEBiuuEnfWAaPedED5kAH9wduJg4UZn4rOm8aDQfwz + S07alHBotePBAnOuqby0tFTcP/iRNaTV68Fg6NYajSaWkG4tgOnenEz1+jNSIWBJmtHK + biHN+ABUyNEUT0mKLRIPWsXTy8h0Ukho2nXWd09bxRP9T/gCqbkZ6oJcNVumivW+RhyE + yV0V2xr7/IVYxygn7E7iXCbhUQ9Td3GQvlvci9Iv3o7f3f7VD79dQxgqoQq/39XgJ+v5 + sED6hih+qVsC18Ey6SaCXxeJRHH4/xYwp6ZhSU1FZnX7mr723q4VrdiSaBW7VGMSn5BX + YhL/lwPjBzyKaTemUUxnML2N6TymC3gjg0mHyYNpWnzih20wSRN8lr+Sb5vCr5jCS2u9 + 7P6OKe2rpvBdU3jxW9bl80v/E3PZeDdNab95Cr92Cn/rFL53Cr9O5P8fJ4dYIAplbmRz + dHJlYW0KZW5kb2JqCjMxIDAgb2JqCjU5MDEKZW5kb2JqCjMyIDAgb2JqCjw8IC9UeXBl + IC9Gb250RGVzY3JpcHRvciAvQXNjZW50IDc3MCAvQ2FwSGVpZ2h0IDcxNyAvRGVzY2Vu + dCAtMjMwIC9GbGFncyAzMgovRm9udEJCb3ggWy05NTEgLTQ4MSAxNDQ1IDExMjJdIC9G + b250TmFtZSAvR0lTVklCK0hlbHZldGljYSAvSXRhbGljQW5nbGUgMAovU3RlbVYgMCAv + TWF4V2lkdGggMTUwMCAvWEhlaWdodCA2MzcgL0ZvbnRGaWxlMiAzMCAwIFIgPj4KZW5k + b2JqCjMzIDAgb2JqClsgNTU2IDU1NiA1MDAgMCA1NTYgMjc4IDU1NiAwIDIyMiAwIDAg + MjIyIDgzMyA1NTYgNTU2IDAgMCAzMzMgNTAwIDI3OCAwIDAKMCAwIDUwMCBdCmVuZG9i + agoxNCAwIG9iago8PCAvVHlwZSAvRm9udCAvU3VidHlwZSAvVHJ1ZVR5cGUgL0Jhc2VG + b250IC9HSVNWSUIrSGVsdmV0aWNhIC9Gb250RGVzY3JpcHRvcgozMiAwIFIgL1dpZHRo + cyAzMyAwIFIgL0ZpcnN0Q2hhciA5NyAvTGFzdENoYXIgMTIxIC9FbmNvZGluZyAvTWFj + Um9tYW5FbmNvZGluZwo+PgplbmRvYmoKMzQgMCBvYmoKKE1hYyBPUyBYIDEwLjYuNCBR + dWFydHogUERGQ29udGV4dCkKZW5kb2JqCjM1IDAgb2JqCihEOjIwMTAwNzEzMDQxODIy + WjAwJzAwJykKZW5kb2JqCjEgMCBvYmoKPDwgL1Byb2R1Y2VyIDM0IDAgUiAvQ3JlYXRp + b25EYXRlIDM1IDAgUiAvTW9kRGF0ZSAzNSAwIFIgPj4KZW5kb2JqCnhyZWYKMCAzNgow + MDAwMDAwMDAwIDY1NTM1IGYgCjAwMDAwMjgyNzAgMDAwMDAgbiAKMDAwMDAyMTQwMiAw + MDAwMCBuIAowMDAwMDAxNTc0IDAwMDAwIG4gCjAwMDAwMjEyMzkgMDAwMDAgbiAKMDAw + MDAwMDAyMiAwMDAwMCBuIAowMDAwMDAxNTU0IDAwMDAwIG4gCjAwMDAwMDE2ODQgMDAw + MDAgbiAKMDAwMDAxNzUxNCAwMDAwMCBuIAowMDAwMDAxODU3IDAwMDAwIG4gCjAwMDAw + MDYzMjUgMDAwMDAgbiAKMDAwMDAwNjM0NiAwMDAwMCBuIAowMDAwMDA2ODMwIDAwMDAw + IG4gCjAwMDAwMTgzNzggMDAwMDAgbiAKMDAwMDAyODAwMSAwMDAwMCBuIAowMDAwMDIx + MjAyIDAwMDAwIG4gCjAwMDAwMDY4NTAgMDAwMDAgbiAKMDAwMDAxMzU5MiAwMDAwMCBu + IAowMDAwMDEzNjEzIDAwMDAwIG4gCjAwMDAwMTY2MzMgMDAwMDAgbiAKMDAwMDAxNjY1 + NCAwMDAwMCBuIAowMDAwMDE3NDk0IDAwMDAwIG4gCjAwMDAwMTc1NTAgMDAwMDAgbiAK + MDAwMDAxODM1OCAwMDAwMCBuIAowMDAwMDE4NDE1IDAwMDAwIG4gCjAwMDAwMjExODEg + MDAwMDAgbiAKMDAwMDAyMTMyMiAwMDAwMCBuIAowMDAwMDIxNTY4IDAwMDAwIG4gCjAw + MDAwMjE0NTAgMDAwMDAgbiAKMDAwMDAyMTU0NiAwMDAwMCBuIAowMDAwMDIxNjY0IDAw + MDAwIG4gCjAwMDAwMjc2NTUgMDAwMDAgbiAKMDAwMDAyNzY3NiAwMDAwMCBuIAowMDAw + MDI3OTAxIDAwMDAwIG4gCjAwMDAwMjgxNzYgMDAwMDAgbiAKMDAwMDAyODIyOCAwMDAw + MCBuIAp0cmFpbGVyCjw8IC9TaXplIDM2IC9Sb290IDI2IDAgUiAvSW5mbyAxIDAgUiAv + SUQgWyA8NjJjM2U5NGRkYzI0MzQ5YjFiNDQ0MjFmMDRiOTg4MzE+Cjw2MmMzZTk0ZGRj + MjQzNDliMWI0NDQyMWYwNGI5ODgzMT4gXSA+PgpzdGFydHhyZWYKMjgzNDUKJSVFT0YK + MSAwIG9iago8PC9BdXRob3IgKEZyYW5rIERlbGxhZXJ0KS9DcmVhdGlvbkRhdGUgKEQ6 + MjAxMDA3MTMwNDA3MDBaKS9DcmVhdG9yIChPbW5pR3JhZmZsZSA1LjIuMikvTW9kRGF0 + ZSAoRDoyMDEwMDcxMzA0MTcwMFopL1Byb2R1Y2VyIDM0IDAgUiA+PgplbmRvYmoKeHJl + ZgoxIDEKMDAwMDAyOTIyMyAwMDAwMCBuIAp0cmFpbGVyCjw8L0lEIFs8NjJjM2U5NGRk + YzI0MzQ5YjFiNDQ0MjFmMDRiOTg4MzE+IDw2MmMzZTk0ZGRjMjQzNDliMWI0NDQyMWYw + NGI5ODgzMT5dIC9JbmZvIDEgMCBSIC9QcmV2IDI4MzQ1IC9Sb290IDI2IDAgUiAvU2l6 + ZSAzNj4+CnN0YXJ0eHJlZgoyOTM3MwolJUVPRgo= + + QuickLookThumbnail + + TU0AKgAABZyAP+BP8AQWDQeEQmFQuGQ2HQ+IRGJROFwOCQZsRkAQOKR2PR+QSGJxaDtC + TAB+SkABmWAABS8APiZAAETWVy2RTmdTuIySDLigAB20MABSjRuBAB90sABKnAAd1GeV + OqVSfQVn1kACOuAAHV+q2GxWODVcANe0AAU2uyRiNRy2wgA3MACi7XGEWa0Ne1Wy43u+ + imEYCXgIAAXEAB/YsAPrHAAVZGIYC14K8WW4QXKX625vLQZwaEAOPSS6YUt9gB+6sAEP + XZO05XL5ikwbPXjb7Oz7HOZe9bzP2SMtikReHPnkAAD8uYzPCgADdGGXMA3W77rf3zZb + qJtrvAATeGUSpveUACH0YfE9yHdnA+yHPb5AB0/Xz+mD0N2gD5PYAA7AD4Ia9ztwEhR0 + QQ9QCgACcGoYbsIAADEJgABcLQMg8CN7DCDO8bT7hDBSGHrEgAGrE4ABfFTlOZAUNODA + 0SHqAB1RrECKDdHIACVHgACRH8DRfDiCnPIsWAOpqnoO4bioQ5B8gAeUpAACsqpA6jrB + QikhSHDwABFMAAAJMaEGnMzwPEeE1AAC02ybLDMoLLEsIO3KJS4+EZAAdc+AAEE/oYY9 + BAAd1CgBNR4AActFgADVHREmR8K8sEEHQAA00wwbgS3OM7PZIpzyPJIJIZT03rojk6No + AFVM1TaR07V74S9MARTFMiF1MsVdPbWLtQ2uM9T4dc/UAh8mTiiFhypKyHywu0tVg2tX + V/GC41BUSnVIuLQnBYoQLxPDLmzcitq7MYCLwbl1gAEl3XDXz3riel6KEogP3w7kmWhe + Fpt3aq8EFgQAB/guCYMuL+gAM+GAASWHgACOJLHcSxlTi4ACfjSaJs3Ra4+AAgZFCsLr + Fislrffy8We663OJZKxZZaNe39XlqQlChw51SYHAAeef59oCjApQlDAZo+I4nAAO01gE + 603CBu6CecRaOBgAHjrKiqPRwNVLWUB3jAqEsBdAAGbtFvsUxgIbaAFsQsBecAwADym8 + AAhbzpt5NtTZzb+AFKtU1iONIcYACJxIAAfxmv6dmjjX/vmybA3HK5sqnMQzsVgZdJqP + ykeWeIhmSEWRlSFz1uKKdKh+TrCcXYgBtoIdG3Wo3MEd+8jzSeayeMoynpd9I1fi49eq + qOS8EvmNMwy4y88ITd3p/HsvRZygABXt6SCKx2wZXw4zjfj85ay23oegAHZ9m1LFuwAE + z+QAET+vqb76zuS89EQsRBZC3TuRIYYsfyTQBwHdIXR4yd3zIGcE2ZobjgAAngoqxVBS + XBITbonBlRz3quTIg8gvCkVFKMK47ohaghjwleyoUdxCEqgVT2n1qyh01gXhwAAKcO29 + tjhDA1IbuANxDY4AiHrnWwwCVbEd88SXeOVQE+wdjgx+gAhwBd0zKYBMxgUy0nsQEOGo + AAN+MiaHpkThI9h3KQYwJDIKt1ZkMnVkPjgxJ7zjAHxsZrFBDg74/AAHvIFRqjyFs/ao + stWsboRICNXFVdY3IJwVIW7iDTJG5JDkWhwQsmwAA2k8AAIsoQACqlIACPw72FsNjcbS + J7+S4wBIYtg+o6QAAwlsAAYEuQAAsl41tohB3WmzkyVV3pO5ilWja5Z/MKjGmPdCAABM + 0QAA4moACGquY+FkmG5lyr8EnyAkESwDLi3GvbAVBKHz5Y9yuM7Nmbk7JtTJlfFplcXW + Zm+nlKufRO5tz7n9Axf0sJ/0DJEWaghD41RDA3QcirMKGELoTESh5eaHUTIRRGhdFpWU + aIbRijhBaDUfIMjkNwAAo0nAAEGlVFqQoYoEQ6R5N5xxzIdMGfDqEMTHJzTonlLUDG3m + eMSoUFjqnRAMqIHlSTYTwYpRVARt4CAAXINl51FCCQLIbTyflTj4VaJBV4nNPkBUvLJT + Z+9Iq0EOICAADwEAAAMAAAABACUAAAEBAAMAAAABAEMAAAECAAMAAAAEAAAGVgEDAAMA + AAABAAUAAAEGAAMAAAABAAIAAAERAAQAAAABAAAACAESAAMAAAABAAEAAAEVAAMAAAAB + AAQAAAEWAAMAAAABAEMAAAEXAAQAAAABAAAFlAEcAAMAAAABAAEAAAE9AAMAAAABAAIA + AAFSAAMAAAABAAEAAAFTAAMAAAAEAAAGXodzAAcAAAzEAAAGZgAAAAAACAAIAAgACAAB + AAEAAQABAAAMxGFwcGwCEAAAbW50clJHQiBYWVogB9oABgAcAAsAEQAQYWNzcEFQUEwA + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAPbWAAEAAAAA0y1hcHBsAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAARZGVzYwAAAVAAAABiZHNj + bQAAAbQAAAD+Y3BydAAAArQAAADQd3RwdAAAA4QAAAAUclhZWgAAA5gAAAAUZ1hZWgAA + A6wAAAAUYlhZWgAAA8AAAAAUclRSQwAAA9QAAAgMYWFyZwAAC+AAAAAgdmNndAAADAAA + AAAwbmRpbgAADDAAAAA+Y2hhZAAADHAAAAAsbW1vZAAADJwAAAAoYlRSQwAAA9QAAAgM + Z1RSQwAAA9QAAAgMYWFiZwAAC+AAAAAgYWFnZwAAC+AAAAAgZGVzYwAAAAAAAAAIRGlz + cGxheQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAG1sdWMAAAAAAAAAEgAAAAxu + bE5MAAAAFgAAAOhkYURLAAAAFgAAAOhwbFBMAAAAFgAAAOhlblVTAAAAFgAAAOhuYk5P + AAAAFgAAAOhmckZSAAAAFgAAAOhwdEJSAAAAFgAAAOhwdFBUAAAAFgAAAOh6aENOAAAA + FgAAAOhlc0VTAAAAFgAAAOhqYUpQAAAAFgAAAOhydVJVAAAAFgAAAOhzdlNFAAAAFgAA + AOh6aFRXAAAAFgAAAOhkZURFAAAAFgAAAOhmaUZJAAAAFgAAAOhpdElUAAAAFgAAAOhr + b0tSAAAAFgAAAOgARABFAEwATAAgADIAMAAwADAARgBQAAB0ZXh0AAAAAENvcHlyaWdo + dCBBcHBsZSwgSW5jLiwgMjAxMAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAWFlaIAAAAAAAAPPP + AAEAAAABGGJYWVogAAAAAAAAeFsAAEAZAAAD91hZWiAAAAAAAABWWgAAntIAABfUWFla + IAAAAAAAACghAAAhFQAAt2JjdXJ2AAAAAAAABAAAAAAFAAoADwAUABkAHgAjACgALQAy + ADYAOwBAAEUASgBPAFQAWQBeAGMAaABtAHIAdwB8AIEAhgCLAJAAlQCaAJ8AowCoAK0A + sgC3ALwAwQDGAMsA0ADVANsA4ADlAOsA8AD2APsBAQEHAQ0BEwEZAR8BJQErATIBOAE+ + AUUBTAFSAVkBYAFnAW4BdQF8AYMBiwGSAZoBoQGpAbEBuQHBAckB0QHZAeEB6QHyAfoC + AwIMAhQCHQImAi8COAJBAksCVAJdAmcCcQJ6AoQCjgKYAqICrAK2AsECywLVAuAC6wL1 + AwADCwMWAyEDLQM4A0MDTwNaA2YDcgN+A4oDlgOiA64DugPHA9MD4APsA/kEBgQTBCAE + LQQ7BEgEVQRjBHEEfgSMBJoEqAS2BMQE0wThBPAE/gUNBRwFKwU6BUkFWAVnBXcFhgWW + BaYFtQXFBdUF5QX2BgYGFgYnBjcGSAZZBmoGewaMBp0GrwbABtEG4wb1BwcHGQcrBz0H + TwdhB3QHhgeZB6wHvwfSB+UH+AgLCB8IMghGCFoIbgiCCJYIqgi+CNII5wj7CRAJJQk6 + CU8JZAl5CY8JpAm6Cc8J5Qn7ChEKJwo9ClQKagqBCpgKrgrFCtwK8wsLCyILOQtRC2kL + gAuYC7ALyAvhC/kMEgwqDEMMXAx1DI4MpwzADNkM8w0NDSYNQA1aDXQNjg2pDcMN3g34 + DhMOLg5JDmQOfw6bDrYO0g7uDwkPJQ9BD14Peg+WD7MPzw/sEAkQJhBDEGEQfhCbELkQ + 1xD1ERMRMRFPEW0RjBGqEckR6BIHEiYSRRJkEoQSoxLDEuMTAxMjE0MTYxODE6QTxRPl + FAYUJxRJFGoUixStFM4U8BUSFTQVVhV4FZsVvRXgFgMWJhZJFmwWjxayFtYW+hcdF0EX + ZReJF64X0hf3GBsYQBhlGIoYrxjVGPoZIBlFGWsZkRm3Gd0aBBoqGlEadxqeGsUa7BsU + GzsbYxuKG7Ib2hwCHCocUhx7HKMczBz1HR4dRx1wHZkdwx3sHhYeQB5qHpQevh7pHxMf + Ph9pH5Qfvx/qIBUgQSBsIJggxCDwIRwhSCF1IaEhziH7IiciVSKCIq8i3SMKIzgjZiOU + I8Ij8CQfJE0kfCSrJNolCSU4JWgllyXHJfcmJyZXJocmtyboJxgnSSd6J6sn3CgNKD8o + cSiiKNQpBik4KWspnSnQKgIqNSpoKpsqzysCKzYraSudK9EsBSw5LG4soizXLQwtQS12 + Last4S4WLkwugi63Lu4vJC9aL5Evxy/+MDUwbDCkMNsxEjFKMYIxujHyMioyYzKbMtQz + DTNGM38zuDPxNCs0ZTSeNNg1EzVNNYc1wjX9Njc2cjauNuk3JDdgN5w31zgUOFA4jDjI + OQU5Qjl/Obw5+To2OnQ6sjrvOy07azuqO+g8JzxlPKQ84z0iPWE9oT3gPiA+YD6gPuA/ + IT9hP6I/4kAjQGRApkDnQSlBakGsQe5CMEJyQrVC90M6Q31DwEQDREdEikTORRJFVUWa + Rd5GIkZnRqtG8Ec1R3tHwEgFSEtIkUjXSR1JY0mpSfBKN0p9SsRLDEtTS5pL4kwqTHJM + uk0CTUpNk03cTiVObk63TwBPSU+TT91QJ1BxULtRBlFQUZtR5lIxUnxSx1MTU19TqlP2 + VEJUj1TbVShVdVXCVg9WXFapVvdXRFeSV+BYL1h9WMtZGllpWbhaB1pWWqZa9VtFW5Vb + 5Vw1XIZc1l0nXXhdyV4aXmxevV8PX2Ffs2AFYFdgqmD8YU9homH1YklinGLwY0Njl2Pr + ZEBklGTpZT1lkmXnZj1mkmboZz1nk2fpaD9olmjsaUNpmmnxakhqn2r3a09rp2v/bFds + r20IbWBtuW4SbmtuxG8eb3hv0XArcIZw4HE6cZVx8HJLcqZzAXNdc7h0FHRwdMx1KHWF + deF2Pnabdvh3VnezeBF4bnjMeSp5iXnnekZ6pXsEe2N7wnwhfIF84X1BfaF+AX5ifsJ/ + I3+Ef+WAR4CogQqBa4HNgjCCkoL0g1eDuoQdhICE44VHhauGDoZyhteHO4efiASIaYjO + iTOJmYn+imSKyoswi5aL/IxjjMqNMY2Yjf+OZo7OjzaPnpAGkG6Q1pE/kaiSEZJ6kuOT + TZO2lCCUipT0lV+VyZY0lp+XCpd1l+CYTJi4mSSZkJn8mmia1ZtCm6+cHJyJnPedZJ3S + nkCerp8dn4uf+qBpoNihR6G2oiailqMGo3aj5qRWpMelOKWpphqmi6b9p26n4KhSqMSp + N6mpqhyqj6sCq3Wr6axcrNCtRK24ri2uoa8Wr4uwALB1sOqxYLHWskuywrM4s660JbSc + tRO1irYBtnm28Ldot+C4WbjRuUq5wro7urW7LrunvCG8m70VvY++Cr6Evv+/er/1wHDA + 7MFnwePCX8Lbw1jD1MRRxM7FS8XIxkbGw8dBx7/IPci8yTrJuco4yrfLNsu2zDXMtc01 + zbXONs62zzfPuNA50LrRPNG+0j/SwdNE08bUSdTL1U7V0dZV1tjXXNfg2GTY6Nls2fHa + dtr724DcBdyK3RDdlt4c3qLfKd+v4DbgveFE4cziU+Lb42Pj6+Rz5PzlhOYN5pbnH+ep + 6DLovOlG6dDqW+rl63Dr++yG7RHtnO4o7rTvQO/M8Fjw5fFy8f/yjPMZ86f0NPTC9VD1 + 3vZt9vv3ivgZ+Kj5OPnH+lf65/t3/Af8mP0p/br+S/7c/23//3BhcmEAAAAAAAMAAAAC + ZmYAAPKnAAANWQAAE9AAAArAdmNndAAAAAAAAAABAADhSAAAAAAAAQAAAADhSAAAAAAA + AQAAAADhSAAAAAAAAQAAbmRpbgAAAAAAAAA2AAChQAAAVwAAAEzAAACXAAAAJkAAABwA + AABQAAAAVAAAAoAAAAKAAAACgAAAAAAAAAAAAHNmMzIAAAAAAAEMGgAABcD///L/AAAH + YAAA/c7///uY///9lgAAA/QAAL9ObW1vZAAAAAAAABCsAACgAzA0Rky6bz8AAAAAAAAA + AAAAAAAAAAAAAA== + + ReadOnly + NO + RowAlign + 1 + RowSpacing + 36 + SheetTitle + Canvas 1 + SmartAlignmentGuidesActive + YES + SmartDistanceGuidesActive + YES + UniqueID + 1 + UseEntirePage + + VPages + 1 + WindowInfo + + CurrentSheet + 0 + ExpandedCanvases + + FitInWindow + + Frame + {{470, 267}, {700, 891}} + ListView + + OutlineWidth + 142 + RightSidebar + + Sidebar + + SidebarWidth + 138 + VisibleRegion + {{1, 1}, {558, 782}} + Zoom + 1 + ZoomValues + + + Canvas 1 + 0.0 + 1 + + + + saveQuickLookFiles + YES + + diff --git a/doc/images/gtsam-structure.pdf b/doc/images/gtsam-structure.pdf new file mode 100644 index 000000000..37c2ed3c1 Binary files /dev/null and b/doc/images/gtsam-structure.pdf differ diff --git a/doc/images/n-steps.pdf b/doc/images/n-steps.pdf new file mode 100644 index 000000000..165accc2d Binary files /dev/null and b/doc/images/n-steps.pdf differ diff --git a/doc/images/n-steps.png b/doc/images/n-steps.png new file mode 100644 index 000000000..988fe5dfc Binary files /dev/null and b/doc/images/n-steps.png differ diff --git a/doc/macros.lyx b/doc/macros.lyx new file mode 100644 index 000000000..1e57e1675 --- /dev/null +++ b/doc/macros.lyx @@ -0,0 +1,294 @@ +#LyX 1.6.5 created this file. For more info see http://www.lyx.org/ +\lyxformat 345 +\begin_document +\begin_header +\textclass article +\use_default_options true +\language english +\inputencoding auto +\font_roman default +\font_sans default +\font_typewriter default +\font_default_family default +\font_sc false +\font_osf false +\font_sf_scale 100 +\font_tt_scale 100 + +\graphics default +\paperfontsize default +\use_hyperref false +\papersize default +\use_geometry false +\use_amsmath 1 +\use_esint 1 +\cite_engine basic +\use_bibtopic false +\paperorientation portrait +\secnumdepth 3 +\tocdepth 3 +\paragraph_separation indent +\defskip medskip +\quotes_language english +\papercolumns 1 +\papersides 1 +\paperpagestyle default +\tracking_changes false +\output_changes false +\author "" +\author "" +\end_header + +\begin_body + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +Derivatives +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}} +{\frac{\partial#1}{\partial#2}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\at}[2]{#1\biggr\rvert_{#2}} +{#1\biggr\rvert_{#2}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} } +{\at{\deriv{#1}{#2}}{#3}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +Lie Groups +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\xhat}{\hat{x}} +{\hat{x}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\yhat}{\hat{y}} +{\hat{y}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Ad}[1]{Ad_{#1}} +{Ad_{#1}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\define}{\stackrel{\Delta}{=}} +{\stackrel{\Delta}{=}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\gg}{\mathfrak{g}} +{\mathfrak{g}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Rn}{\mathbb{R}^{n}} +{\mathbb{R}^{n}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SO(2) +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}} +{\mathfrak{\mathbb{R}^{2}}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\SOtwo}{SO(2)} +{SO(2)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\sotwo}{\mathfrak{so(2)}} +{\mathfrak{so(2)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\that}{\hat{\theta}} +{\hat{\theta}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\skew}[1]{[#1]_{+}} +{[#1]_{+}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SE(2) +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\SEtwo}{SE(2)} +{SE(2)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\setwo}{\mathfrak{se(2)}} +{\mathfrak{se(2)}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SO(3) +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rthree}{\mathfrak{\mathbb{R}^{3}}} +{\mathfrak{\mathbb{R}^{3}}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\SOthree}{SO(3)} +{SO(3)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\sothree}{\mathfrak{so(3)}} +{\mathfrak{so(3)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\what}{\hat{\omega}} +{\hat{\omega}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Skew}[1]{[#1]_{\times}} +{[#1]_{\times}} +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SE(3) +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\Rsix}{\mathfrak{\mathbb{R}^{6}}} +{\mathfrak{\mathbb{R}^{6}}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\SEthree}{SE(3)} +{SE(3)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\sethree}{\mathfrak{se(3)}} +{\mathfrak{se(3)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\xihat}{\hat{\xi}} +{\hat{\xi}} +\end_inset + + +\end_layout + +\end_body +\end_document diff --git a/doc/math.lyx b/doc/math.lyx new file mode 100644 index 000000000..878a7b167 --- /dev/null +++ b/doc/math.lyx @@ -0,0 +1,5746 @@ +#LyX 2.0 created this file. For more info see http://www.lyx.org/ +\lyxformat 413 +\begin_document +\begin_header +\textclass article +\use_default_options false +\begin_modules +eqs-within-sections +figs-within-sections +theorems-ams-bytype +\end_modules +\maintain_unincluded_children false +\language english +\language_package default +\inputencoding auto +\fontencoding global +\font_roman times +\font_sans default +\font_typewriter default +\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 +\bibtex_command default +\index_command default +\paperfontsize 12 +\spacing single +\use_hyperref false +\papersize default +\use_geometry true +\use_amsmath 1 +\use_esint 0 +\use_mhchem 1 +\use_mathdots 1 +\cite_engine basic +\use_bibtopic false +\use_indices false +\paperorientation portrait +\suppress_date false +\use_refstyle 0 +\index Index +\shortcut idx +\color #008000 +\end_index +\leftmargin 1in +\topmargin 1in +\rightmargin 1in +\bottommargin 1in +\secnumdepth 3 +\tocdepth 3 +\paragraph_separation indent +\paragraph_indentation default +\quotes_language english +\papercolumns 1 +\papersides 1 +\paperpagestyle default +\tracking_changes false +\output_changes false +\html_math_output 0 +\html_css_as_file 0 +\html_be_strict false +\end_header + +\begin_body + +\begin_layout Title +Derivatives and Differentials +\end_layout + +\begin_layout Author +Frank Dellaert +\end_layout + +\begin_layout Standard +\begin_inset Box Frameless +position "t" +hor_pos "c" +has_inner_box 1 +inner_pos "t" +use_parbox 0 +use_makebox 0 +width "100col%" +special "none" +height "1in" +height_special "totalheight" +status collapsed + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\SqrMah}[3]{\Vert{#1}-{#2}\Vert_{#3}^{2}} +{\Vert{#1}-{#2}\Vert_{#3}^{2}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\SqrZMah}[2]{\Vert{#1}\Vert_{#2}^{2}} +{\Vert{#1}\Vert_{#2}^{2}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\Rone}{\mathbb{R}} +{\mathbb{R}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\Reals}[1]{\mathbb{R}^{#1}} +{\mathbb{R}^{#1}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\OneD}[1]{\Reals{#1}\rightarrow\Rone} +{\Reals{#1}\rightarrow\Rone} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\Multi}[2]{\Reals{#1}\rightarrow\Reals{#2}} +{\Reals{#1}\rightarrow\Reals{#2}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\Man}{\mathcal{M}} +{\mathcal{M}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +Derivatives +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}} +{\frac{\partial#1}{\partial#2}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\at}[2]{#1\biggr\rvert_{#2}} +{#1\biggr\rvert_{#2}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} } +{\at{\deriv{#1}{#2}}{#3}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +Lie Groups +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\xhat}{\hat{x}} +{\hat{x}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\yhat}{\hat{y}} +{\hat{y}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Ad}[1]{Ad_{#1}} +{Ad_{#1}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\define}{\stackrel{\Delta}{=}} +{\stackrel{\Delta}{=}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\gg}{\mathfrak{g}} +{\mathfrak{g}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Rn}{\mathbb{R}^{n}} +{\mathbb{R}^{n}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SO(2) +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\Rtwo}{\mathbb{R}^{2}} +{\mathbb{R}^{2}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\SOtwo}{SO(2)} +{SO(2)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\sotwo}{\mathfrak{so(2)}} +{\mathfrak{so(2)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\that}{\hat{\theta}} +{\hat{\theta}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\skew}[1]{[#1]_{+}} +{[#1]_{+}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SE(2) +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\SEtwo}{SE(2)} +{SE(2)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\setwo}{\mathfrak{se(2)}} +{\mathfrak{se(2)}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SO(3) +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\Rthree}{\mathbb{R}^{3}} +{\mathbb{R}^{3}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\SOthree}{SO(3)} +{SO(3)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\sothree}{\mathfrak{so(3)}} +{\mathfrak{so(3)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\what}{\hat{\omega}} +{\hat{\omega}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\Skew}[1]{[#1]_{\times}} +{[#1]_{\times}} +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset Note Comment +status open + +\begin_layout Plain Layout +SE(3) +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Plain Layout +\begin_inset FormulaMacro +\newcommand{\Rsix}{\mathbb{R}^{6}} +{\mathbb{R}^{6}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\SEthree}{SE(3)} +{SE(3)} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\sethree}{\mathfrak{se(3)}} +{\mathfrak{se(3)}} +\end_inset + + +\begin_inset FormulaMacro +\newcommand{\xihat}{\hat{\xi}} +{\hat{\xi}} +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Part +Theory +\end_layout + +\begin_layout Section +Optimization +\end_layout + +\begin_layout Standard +We will be concerned with minimizing a non-linear least squares objective + of the form +\begin_inset Formula +\begin{equation} +x^{*}=\arg\min_{x}\SqrMah{h(x)}z{\Sigma}\label{eq:objective} +\end{equation} + +\end_inset + +where +\begin_inset Formula $x\in\Man$ +\end_inset + + is a point on an +\begin_inset Formula $n$ +\end_inset + +-dimensional manifold (which could be +\begin_inset Formula $\Reals n$ +\end_inset + +, an n-dimensional Lie group +\begin_inset Formula $G$ +\end_inset + +, or a general manifold +\begin_inset Formula $\Man)$ +\end_inset + +, +\begin_inset Formula $z\in\Reals m$ +\end_inset + + is an observed measurement, +\begin_inset Formula $h:\Man\rightarrow\Reals m$ +\end_inset + + is a measurement function that predicts +\begin_inset Formula $z$ +\end_inset + + from +\begin_inset Formula $x$ +\end_inset + +, and +\begin_inset Formula $\SqrZMah e{\Sigma}\define e^{T}\Sigma^{-1}e$ +\end_inset + + is the squared Mahalanobis distance with covariance +\begin_inset Formula $\Sigma$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +To minimize +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:objective" + +\end_inset + + we need a notion of how the non-linear measurement function +\begin_inset Formula $h(x)$ +\end_inset + + behaves in the neighborhood of a linearization point +\begin_inset Formula $a$ +\end_inset + +. + Loosely speaking, we would like to define an +\begin_inset Formula $m\times n$ +\end_inset + + Jacobian matrix +\begin_inset Formula $H_{a}$ +\end_inset + + such that +\begin_inset Formula +\begin{equation} +h(a\oplus\xi)\approx h(a)+H_{a}\xi\label{eq:LocalBehavior} +\end{equation} + +\end_inset + +with +\begin_inset Formula $\xi\in\Reals n$ +\end_inset + +, and the operation +\begin_inset Formula $\oplus$ +\end_inset + + +\begin_inset Quotes eld +\end_inset + +increments +\begin_inset Quotes erd +\end_inset + + +\begin_inset Formula $a\in\Man$ +\end_inset + +. + Below we more formally develop this notion, first for functions from +\begin_inset Formula $\Multi nm$ +\end_inset + +, then for Lie groups, and finally for manifolds. +\end_layout + +\begin_layout Standard +Once equipped with the approximation +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:LocalBehavior" + +\end_inset + +, we can minimize the objective function +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:objective" + +\end_inset + + with respect to +\begin_inset Formula $\delta x$ +\end_inset + + instead: +\begin_inset Formula +\begin{equation} +\xi^{*}=\arg\min_{\xi}\SqrMah{h(a)+H_{a}\xi}z{\Sigma}\label{eq:ApproximateObjective} +\end{equation} + +\end_inset + +This can be done by setting the derivative of +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:ApproximateObjective" + +\end_inset + + to zero, +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout + +\begin_inset Formula +\[ +\frac{1}{2}H_{a}^{T}(h(a)+H_{a}\xi-z)=0 +\] + +\end_inset + + +\end_layout + +\end_inset + + yielding the +\series bold +normal equations +\series default +, +\begin_inset Formula +\[ +H_{a}^{T}H_{a}\xi=H_{a}^{T}\left(z-h(a)\right) +\] + +\end_inset + +which can be solved using Cholesky factorization. + Of course, we might have to iterate this multiple times, and use a trust-region + method to bound +\begin_inset Formula $\xi$ +\end_inset + + when the approximation +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:LocalBehavior" + +\end_inset + + is not good. +\end_layout + +\begin_layout Section +Multivariate Differentiation +\end_layout + +\begin_layout Subsection +Derivatives +\end_layout + +\begin_layout Standard +For a vector space +\begin_inset Formula $\Reals n$ +\end_inset + +, the notion of an increment is just done by vector addition +\begin_inset Formula +\[ +a\oplus\xi\define a+\xi +\] + +\end_inset + +and for the approximation +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:LocalBehavior" + +\end_inset + + we will use a Taylor expansion using multivariate differentiation. + However, loosely following +\begin_inset CommandInset citation +LatexCommand cite +key "Spivak65book" + +\end_inset + +, we use a perhaps unfamiliar way to define derivatives: +\end_layout + +\begin_layout Definition +\begin_inset CommandInset label +LatexCommand label +name "def:differentiable" + +\end_inset + +We define a function +\begin_inset Formula $f:\Multi nm$ +\end_inset + + to be +\series bold +differentiable +\series default + at +\begin_inset Formula $a$ +\end_inset + + if there exists a matrix +\begin_inset Formula $f'(a)\in\Reals{m\times n}$ +\end_inset + + such that +\begin_inset Formula +\[ +\lim_{\delta x\rightarrow0}\frac{\left|f(a)+f'(a)\xi-f(a+\xi)\right|}{\left|\xi\right|}=0 +\] + +\end_inset + +where +\begin_inset Formula $\left|e\right|\define\sqrt{e^{T}e}$ +\end_inset + + is the usual norm. + If +\begin_inset Formula $f$ +\end_inset + + is differentiable, then the matrix +\begin_inset Formula $f'(a)$ +\end_inset + + is called the +\series bold +Jacobian matrix +\series default + of +\begin_inset Formula $f$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +, and the linear map +\begin_inset Formula $Df_{a}:\xi\mapsto f'(a)\xi$ +\end_inset + + is called the +\series bold +derivative +\series default + of +\begin_inset Formula $f$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +. + When no confusion is likely, we use the notation +\begin_inset Formula $F_{a}\define f'(a)$ +\end_inset + + to stress that +\begin_inset Formula $f'(a)$ +\end_inset + + is a matrix. +\end_layout + +\begin_layout Standard +The benefit of using this definition is that it generalizes the notion of + a scalar derivative +\begin_inset Formula $f'(a):\Rone\rightarrow\Rone$ +\end_inset + + to multivariate functions from +\begin_inset Formula $\Multi nm$ +\end_inset + +. + In particular, the derivative +\begin_inset Formula $Df_{a}$ +\end_inset + + maps vector increments +\begin_inset Formula $\xi$ +\end_inset + + on +\begin_inset Formula $a$ +\end_inset + + to increments +\begin_inset Formula $f'(a)\xi$ +\end_inset + + on +\begin_inset Formula $f(a)$ +\end_inset + +, such that this linear map locally approximates +\begin_inset Formula $f$ +\end_inset + +: +\begin_inset Formula +\[ +f(a+\xi)\approx f(a)+f'(a)\xi +\] + +\end_inset + + +\end_layout + +\begin_layout Example +\begin_inset CommandInset label +LatexCommand label +name "ex:projection" + +\end_inset + +The function +\begin_inset Formula $\pi:(x,y,z)\mapsto(x/z,y/z)$ +\end_inset + + projects a 3D point +\begin_inset Formula $(x,y,z)$ +\end_inset + + to the image plane, and has the Jacobian matrix +\begin_inset Formula +\[ +\pi'(x,y,z)=\frac{1}{z}\left[\begin{array}{ccc} +1 & 0 & -x/z\\ +0 & 1 & -y/z +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Properties of Derivatives +\end_layout + +\begin_layout Standard +This notion of a multivariate derivative obeys the usual rules: +\end_layout + +\begin_layout Theorem +(Chain rule) If +\begin_inset Formula $f:\Multi np$ +\end_inset + + is differentiable at +\begin_inset Formula $a$ +\end_inset + + and +\begin_inset Formula $g:\Multi pm$ +\end_inset + + is differentiable at +\begin_inset Formula $f(a)$ +\end_inset + +, +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + then +\begin_inset Formula $D(g\circ f)_{a}=Dg_{f(a)}\circ Df_{a}$ +\end_inset + + and +\end_layout + +\end_inset + + then the Jacobian matrix +\begin_inset Formula $H_{a}$ +\end_inset + + of +\begin_inset Formula $h=g\circ f$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + + is the +\begin_inset Formula $m\times n$ +\end_inset + + matrix product +\begin_inset Formula +\[ +H_{a}=G_{f(a)}F_{a} +\] + +\end_inset + + +\end_layout + +\begin_layout Proof +See +\begin_inset CommandInset citation +LatexCommand cite +key "Spivak65book" + +\end_inset + + +\end_layout + +\begin_layout Example +\begin_inset CommandInset label +LatexCommand label +name "ex:chain-rule" + +\end_inset + +If we follow the projection +\begin_inset Formula $\pi$ +\end_inset + + by a calibration step +\begin_inset Formula $\gamma:(x,y)\mapsto(u_{0}+fx,u_{0}+fy)$ +\end_inset + +, with +\begin_inset Formula +\[ +\gamma'(x,y)=\left[\begin{array}{cc} +f & 0\\ +0 & f +\end{array}\right] +\] + +\end_inset + +then the combined function +\begin_inset Formula $\gamma\circ\pi$ +\end_inset + + has the Jacobian matrix +\begin_inset Formula +\[ +(\gamma\circ\pi)'(x,y)=\frac{f}{z}\left[\begin{array}{ccc} +1 & 0 & -x/z\\ +0 & 1 & -y/z +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Theorem +(Inverse) If +\begin_inset Formula $f:\Multi nn$ +\end_inset + + is differentiable and has a differentiable inverse +\begin_inset Formula $g\define f^{-1}$ +\end_inset + +, then its Jacobian matrix +\begin_inset Formula $G_{a}$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + + is just the inverse of that of +\begin_inset Formula $f$ +\end_inset + +, evaluated at +\begin_inset Formula $g(a)$ +\end_inset + +: +\begin_inset Formula +\[ +G_{a}=\left[F_{g(a)}\right]^{-1} +\] + +\end_inset + + +\end_layout + +\begin_layout Proof +See +\begin_inset CommandInset citation +LatexCommand cite +key "Spivak65book" + +\end_inset + + +\end_layout + +\begin_layout Example +\begin_inset CommandInset label +LatexCommand label +name "ex:inverse" + +\end_inset + +The function +\begin_inset Formula $f:(x,y)\mapsto(x^{2},xy)$ +\end_inset + + has the Jacobian matrix +\end_layout + +\begin_layout Example +\begin_inset Formula +\[ +F_{(x,y)}=\left[\begin{array}{cc} +2x & 0\\ +y & x +\end{array}\right] +\] + +\end_inset + +and, for +\begin_inset Formula $x\geq0$ +\end_inset + +, its inverse is the function +\begin_inset Formula $g:(x,y)\mapsto(x^{1/2},x^{-1/2}y)$ +\end_inset + + with the Jacobian matrix +\begin_inset Formula +\[ +G_{(x,y)}=\frac{1}{2}\left[\begin{array}{cc} +x^{-1/2} & 0\\ +-x^{-3/2}y & 2x^{-1/2} +\end{array}\right] +\] + +\end_inset + +It is easily verified that +\begin_inset Formula +\[ +g'(a,b)f'(a^{1/2},a^{-1/2}b)=\frac{1}{2}\left[\begin{array}{cc} +a^{-1/2} & 0\\ +-a^{-3/2}b & 2a^{-1/2} +\end{array}\right]\left[\begin{array}{cc} +2a^{1/2} & 0\\ +a^{-1/2}b & a^{1/2} +\end{array}\right]=\left[\begin{array}{cc} +1 & 0\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Problem +Verify the above for +\begin_inset Formula $(a,b)=(4,6)$ +\end_inset + +. + Sketch the situation graphically to get insight. +\end_layout + +\begin_layout Subsection +Computing Multivariate Derivatives +\end_layout + +\begin_layout Standard +Computing derivatives is made easy by defining the concept of a partial + derivative: +\end_layout + +\begin_layout Definition +For +\begin_inset Formula $f:\OneD n$ +\end_inset + +, the +\series bold +partial derivative +\series default + of +\begin_inset Formula $f$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +, +\series bold + +\series default + +\begin_inset Formula +\[ +D_{j}f(a)\define\lim_{h\rightarrow0}\frac{f\left(a^{1},\ldots,a^{j}+h,\ldots,a^{n}\right)-f\left(a^{1},\ldots,a^{n}\right)}{h} +\] + +\end_inset + +which is the ordinary derivative of the scalar function +\begin_inset Formula $g(x)\define f\left(a^{1},\ldots,x,\ldots,a^{n}\right)$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +Using this definition, one can show that the Jacobian matrix +\begin_inset Formula $F_{a}$ +\end_inset + + of a differentiable +\emph on +multivariate +\emph default + function +\begin_inset Formula $f:\Multi nm$ +\end_inset + + consists simply of the +\begin_inset Formula $m\times n$ +\end_inset + + partial derivatives +\begin_inset Formula $D_{j}f^{i}(a)$ +\end_inset + +, evaluated at +\begin_inset Formula $a\in\Reals n$ +\end_inset + +: +\begin_inset Formula +\[ +F_{a}=\left[\begin{array}{ccc} +D_{1}f^{1}(a) & \cdots & D_{n}f^{1}(a)\\ +\vdots & \ddots & \vdots\\ +D_{1}f^{m}(a) & \ldots & D_{n}f^{m}(a) +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Problem +Verify the derivatives in Examples +\begin_inset CommandInset ref +LatexCommand ref +reference "ex:projection" + +\end_inset + + to +\begin_inset CommandInset ref +LatexCommand ref +reference "ex:inverse" + +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +Multivariate Functions on Lie Groups +\end_layout + +\begin_layout Subsection +Lie Groups +\end_layout + +\begin_layout Standard +Lie groups are not as easy to treat as the vector space +\begin_inset Formula $\Reals n$ +\end_inset + + but nevertheless have a lot of structure. + To generalize the concept of the total derivative above we just need to + replace +\begin_inset Formula $a\oplus\xi$ +\end_inset + + in +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:ApproximateObjective" + +\end_inset + + with a suitable operation in the Lie group +\begin_inset Formula $G$ +\end_inset + +. + In particular, the notion of an exponential map allows us to define an + incremental transformation as tracing out a geodesic curve on the group + manifold along a certain +\series bold +tangent vector +\series default + +\begin_inset Formula $\xi$ +\end_inset + +, +\begin_inset Formula +\[ +a\oplus\xi\define a\exp\left(\hat{\xi}\right) +\] + +\end_inset + +with +\begin_inset Formula $\xi\in\Reals n$ +\end_inset + + for an +\begin_inset Formula $n$ +\end_inset + +-dimensional Lie group, +\begin_inset Formula $\hat{\xi}\in\mathfrak{g}$ +\end_inset + + the Lie algebra element corresponding to the vector +\begin_inset Formula $\xi$ +\end_inset + +, and +\begin_inset Formula $\exp\hat{\xi}$ +\end_inset + + the exponential map. + Note that if +\begin_inset Formula $G$ +\end_inset + + is equal to +\begin_inset Formula $\Reals n$ +\end_inset + + then composing with the exponential map +\begin_inset Formula $ae^{\xihat}$ +\end_inset + + is just vector addition +\begin_inset Formula $a+\xi$ +\end_inset + +. +\end_layout + +\begin_layout Example +For the Lie group +\begin_inset Formula $\SOthree$ +\end_inset + + of 3D rotations the vector +\begin_inset Formula $\xi$ +\end_inset + + is denoted as +\begin_inset Formula $\omega$ +\end_inset + + and represents an angular displacement. + The Lie algebra element +\begin_inset Formula $\xihat$ +\end_inset + + is a skew symmetric matrix denoted as +\begin_inset Formula $\Skew{\omega}\in\sothree$ +\end_inset + +, and is given by +\begin_inset Formula +\[ +\Skew{\omega}=\left[\begin{array}{ccc} +0 & -\omega_{z} & \omega_{y}\\ +\omega_{z} & 0 & -\omega_{x}\\ +-\omega_{y} & \omega_{x} & 0 +\end{array}\right] +\] + +\end_inset + +Finally, the increment +\begin_inset Formula $a\oplus\xi=ae^{\xihat}$ +\end_inset + + corresponds to an incremental rotation +\begin_inset Formula $R\oplus\omega=Re^{\Skew{\omega}}$ +\end_inset + +. +\end_layout + +\begin_layout Subsection +Derivatives +\end_layout + +\begin_layout Standard +We can generalize Definition +\begin_inset CommandInset ref +LatexCommand ref +reference "def:differentiable" + +\end_inset + + to map exponential coordinates +\begin_inset Formula $\xi$ +\end_inset + + to increments +\begin_inset Formula $f'(a)\xi$ +\end_inset + + on +\begin_inset Formula $f(a)$ +\end_inset + +, such that the linear map +\begin_inset Formula $Df_{a}$ +\end_inset + + locally approximates a function +\begin_inset Formula $f$ +\end_inset + + from +\begin_inset Formula $G$ +\end_inset + + to +\begin_inset Formula $\Reals m$ +\end_inset + +: +\begin_inset Formula +\[ +f(ae^{\xihat})\approx f(a)+f'(a)\xi +\] + +\end_inset + + +\end_layout + +\begin_layout Definition +We define a function +\begin_inset Formula $f:G\rightarrow\Reals m$ +\end_inset + + to be +\series bold +differentiable +\series default + at +\begin_inset Formula $a\in G$ +\end_inset + + if there exists a matrix +\begin_inset Formula $f'(a)\in\Reals{m\times n}$ +\end_inset + + such that +\begin_inset Formula +\[ +\lim_{\xi\rightarrow0}\frac{\left|f(a)+f'(a)\xi-f(ae^{\hat{\xi}})\right|}{\left|\xi\right|}=0 +\] + +\end_inset + +If +\begin_inset Formula $f$ +\end_inset + + is differentiable, then the matrix +\begin_inset Formula $f'(a)$ +\end_inset + + is called the +\series bold +Jacobian matrix +\series default + of +\begin_inset Formula $f$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +, and the linear map +\begin_inset Formula $Df_{a}:\xi\mapsto f'(a)\xi$ +\end_inset + + is called the +\series bold +derivative +\series default + of +\begin_inset Formula $f$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +. +\end_layout + +\begin_layout Standard +Note that the vectors +\begin_inset Formula $\xi$ +\end_inset + + can be viewed as lying in the tangent space to +\begin_inset Formula $G$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +, but defining this rigorously would take us on a longer tour of differential + geometry. + Informally, +\begin_inset Formula $\xi$ +\end_inset + + is simply the direction, in a local coordinate frame, that is locally tangent + at +\begin_inset Formula $a$ +\end_inset + + to a geodesic curve +\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$ +\end_inset + + traced out by the exponential map, with +\begin_inset Formula $\gamma(0)=a$ +\end_inset + +. +\end_layout + +\begin_layout Subsection +Derivative of an Action +\begin_inset CommandInset label +LatexCommand label +name "sec:Derivatives-of-Actions" + +\end_inset + + +\end_layout + +\begin_layout Standard +The (usual) action of an +\begin_inset Formula $n$ +\end_inset + +-dimensional matrix group +\begin_inset Formula $G$ +\end_inset + + is matrix-vector multiplication on +\begin_inset Formula $\mathbb{R}^{n}$ +\end_inset + +, i.e., +\begin_inset Formula $f:G\times\Reals n\rightarrow\Reals n$ +\end_inset + + with +\begin_inset Formula +\[ +f(T,p)=Tp +\] + +\end_inset + +Since this is a function defined on the product +\begin_inset Formula $G\times\Reals n$ +\end_inset + + the derivative is a linear transformation +\begin_inset Formula $Df:\Multi{2n}n$ +\end_inset + + with +\begin_inset Formula +\[ +Df_{(T,p)}\left(\xi,\delta p\right)=D_{1}f_{(T,p)}\left(\xi\right)+D_{2}f_{(T,p)}\left(\delta p\right) +\] + +\end_inset + + +\end_layout + +\begin_layout Theorem +\begin_inset CommandInset label +LatexCommand label +name "th:Action" + +\end_inset + +The Jacobian matrix of the group action +\begin_inset Formula $f(T,P)=Tp$ +\end_inset + + at +\begin_inset Formula $(T,p)$ +\end_inset + + is given by +\begin_inset Formula +\[ +F_{(T,p)}=\left[\begin{array}{cc} +TH(p) & T\end{array}\right]=T\left[\begin{array}{cc} +H(p) & I_{n}\end{array}\right] +\] + +\end_inset + +with +\begin_inset Formula $H:\Reals n\rightarrow\Reals{n\times n}$ +\end_inset + + a linear mapping that depends on +\begin_inset Formula $p$ +\end_inset + +, and +\begin_inset Formula $I_{n}$ +\end_inset + + the +\begin_inset Formula $n\times n$ +\end_inset + + identity matrix. +\end_layout + +\begin_layout Proof +First, the derivative +\begin_inset Formula $D_{2}f$ +\end_inset + + with respect to in +\begin_inset Formula $p$ +\end_inset + + is easy, as its matrix is simply T: +\begin_inset Formula +\[ +f(T,p+\delta p)=T(p+\delta p)=Tp+T\delta p=f(T,p)+D_{2}f(\delta p) +\] + +\end_inset + +For the derivative +\begin_inset Formula $D_{1}f$ +\end_inset + + with respect to a change in the first argument +\begin_inset Formula $T$ +\end_inset + +, we want +\end_layout + +\begin_layout Proof + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none +\begin_inset Formula +\[ +f(Te^{\hat{\xi}},p)=Te^{\hat{\xi}}p\approx Tp+D_{1}f(\xi) +\] + +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit +Since the matrix exponential is given by the series +\begin_inset Formula $e^{A}=I+A+\frac{A^{2}}{2!}+\frac{A^{3}}{3!}+\ldots$ +\end_inset + + we have, to first order +\begin_inset Formula +\[ +Te^{\hat{\xi}}p\approx T(I+\hat{\xi})p=Tp+T\hat{\xi}p +\] + +\end_inset + + +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +Note also that +\begin_inset Formula +\[ +T\hat{\xi}p=\left(T\hat{\xi}T^{-1}\right)Tp=\left(\Ad T\xihat\right)\left(Tp\right) +\] + +\end_inset + + +\end_layout + +\end_inset + +Hence, we need to show that +\begin_inset Formula +\begin{equation} +\xihat p=H(p)\xi\label{eq:Hp} +\end{equation} + +\end_inset + +with +\begin_inset Formula $H(p)$ +\end_inset + + an +\begin_inset Formula $n\times n$ +\end_inset + + matrix that depends on +\begin_inset Formula $p$ +\end_inset + +. + Expressing the map +\begin_inset Formula $\xi\rightarrow\hat{\xi}$ +\end_inset + + in terms of the Lie algebra generators +\begin_inset Formula $G^{i}$ +\end_inset + +, using tensors and Einstein summation, we have +\begin_inset Formula $\hat{\xi}_{j}^{i}=G_{jk}^{i}\xi^{k}$ +\end_inset + + allowing us to calculate +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none + +\begin_inset Formula $\hat{\xi}p$ +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit + as +\begin_inset Formula +\[ +\left(\hat{\xi}p\right)^{i}=\hat{\xi}_{j}^{i}p^{j}=G_{jk}^{i}\xi^{k}p^{j}=\left(G_{jk}^{i}p^{j}\right)\xi^{k}=H_{k}^{i}(p)\xi^{k} +\] + +\end_inset + + +\end_layout + +\begin_layout Example +For 3D rotations +\begin_inset Formula $R\in\SOthree$ +\end_inset + +, we have +\begin_inset Formula $\hat{\omega}=\Skew{\omega}$ +\end_inset + + and +\begin_inset Formula +\[ +G_{k=1}:\left(\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & -1\\ +0 & 1 & 0 +\end{array}\right)\mbox{}G_{k=2}:\left(\begin{array}{ccc} +0 & 0 & 1\\ +0 & 0 & 0\\ +-1 & 0 & 0 +\end{array}\right)\mbox{ }G_{k=3}:\left(\begin{array}{ccc} +0 & -1 & 0\\ +1 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right) +\] + +\end_inset + + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none +The matrices +\begin_inset Formula $\left(G_{k}^{i}\right)_{j}$ +\end_inset + + are obtained by assembling the +\begin_inset Formula $j^{th}$ +\end_inset + + columns of the generators above, yielding +\begin_inset Formula $H(p)$ +\end_inset + + equal to: +\begin_inset Formula +\[ +\left(\begin{array}{ccc} +0 & 0 & 0\\ +0 & 0 & 1\\ +0 & -1 & 0 +\end{array}\right)p^{1}+\left(\begin{array}{ccc} +0 & 0 & -1\\ +0 & 0 & 0\\ +1 & 0 & 0 +\end{array}\right)p^{2}+\left(\begin{array}{ccc} +0 & 1 & 0\\ +-1 & 0 & 0\\ +0 & 0 & 0 +\end{array}\right)p^{3}=\left(\begin{array}{ccc} +0 & p^{3} & -p^{2}\\ +-p^{3} & 0 & p^{1}\\ +p^{2} & -p^{1} & 0 +\end{array}\right)=\Skew{-p} +\] + +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\noun default +\color inherit +Hence, the Jacobian matrix of +\begin_inset Formula $f(R,p)=Rp$ +\end_inset + + is given by +\begin_inset Formula +\[ +F_{(R,p)}=R\left(\begin{array}{cc} +\Skew{-p} & I_{3}\end{array}\right) +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Derivative of an Inverse Action +\end_layout + +\begin_layout Standard +Applying the action by the inverse of +\begin_inset Formula $T\in G$ +\end_inset + + yields a function +\begin_inset Formula $g:G\times\Reals n\rightarrow\Reals n$ +\end_inset + + defined by +\begin_inset Formula +\[ +g(T,p)=T^{-1}p +\] + +\end_inset + + +\end_layout + +\begin_layout Theorem +\begin_inset CommandInset label +LatexCommand label +name "Th:InverseAction" + +\end_inset + +The Jacobian matrix of the inverse group action +\begin_inset Formula $g(T,p)=T^{-1}p$ +\end_inset + + is given by +\begin_inset Formula +\[ +G_{(T,p)}=\left[\begin{array}{cc} +-H(T^{-1}p) & T^{-1}\end{array}\right] +\] + +\end_inset + +where +\begin_inset Formula $H:\Reals n\rightarrow\Reals{n\times n}$ +\end_inset + + is the same mapping as before. +\end_layout + +\begin_layout Proof +Again, the derivative +\begin_inset Formula $D_{2}g$ +\end_inset + + with respect to in +\begin_inset Formula $p$ +\end_inset + + is easy, the matrix of which is simply +\begin_inset Formula $T^{-1}$ +\end_inset + +: +\begin_inset Formula +\[ +g(T,p+\delta p)=T^{-1}(p+\delta p)=T^{-1}p+T^{-1}\delta p=g(T,p)+D_{2}g(\delta p) +\] + +\end_inset + +Conversely, a change in +\begin_inset Formula $T$ +\end_inset + + yields +\begin_inset Formula +\[ +g(Te^{\xihat},p)=\left(Te^{\xihat}\right)^{-1}p=e^{-\xihat}T^{-1}p +\] + +\end_inset + +Similar to before, if we expand the matrix exponential we get +\begin_inset Formula +\[ +e^{-A}=I-A+\frac{A^{2}}{2!}-\frac{A^{3}}{3!}+\ldots +\] + +\end_inset + +so +\begin_inset Formula +\[ +e^{-\xihat}T^{-1}p\approx(I-\xihat)T^{-1}p=g(T,p)-\xihat\left(T^{-1}p\right) +\] + +\end_inset + + +\end_layout + +\begin_layout Example +For 3D rotations +\begin_inset Formula $R\in\SOthree$ +\end_inset + + we have +\begin_inset Formula $R^{-1}=R^{T}$ +\end_inset + +, +\begin_inset Formula $H(p)=-\Skew p$ +\end_inset + +, and hence the Jacobian matrix of +\begin_inset Formula $g(R,p)=R^{T}p$ +\end_inset + + is given by +\begin_inset Formula +\[ +G_{(R,p)}=\left(\begin{array}{cc} +\Skew{R^{T}p} & R^{T}\end{array}\right) +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +My earlier attempt: because the wedge operator is linear, we have +\end_layout + +\begin_layout Plain Layout +\begin_inset Formula +\begin{eqnarray*} +f(\xi+x) & = & \exp\widehat{\left(\xi+x\right)}\\ + & = & \exp\left(\xihat+\hat{x}\right) +\end{eqnarray*} + +\end_inset + +However, except for commutative Lie groups, it is not true that +\begin_inset Formula $\exp\left(\xihat+\hat{x}\right)=\exp\xihat\exp\hat{x}$ +\end_inset + +. + However, if we expand the matrix exponential to second order and assume + +\begin_inset Formula $x\rightarrow0$ +\end_inset + + we do have +\begin_inset Formula +\[ +\exp\left(\xihat+\hat{x}\right)\approx I+\xihat+\hat{x}+\frac{1}{2}\xihat^{2}+\xhat\xihat +\] + +\end_inset + +Now, if we ask what +\begin_inset Formula $\hat{y}$ +\end_inset + + would effect the same change: +\begin_inset Formula +\begin{eqnarray*} +\exp\xihat\exp\yhat & = & I+\xihat+\hat{x}+\frac{1}{2}\xihat^{2}+\xhat\xihat\\ +\exp\xihat(I+\yhat) & = & I+\xihat+\hat{x}+\frac{1}{2}\xihat^{2}+\xhat\xihat\\ +\left(\exp\xihat\right)\yhat & = & \xhat+\xhat\xihat +\end{eqnarray*} + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Section +Instantaneous Velocity +\end_layout + +\begin_layout Standard +For matrix Lie groups, if we have a matrix +\begin_inset Formula $T_{b}^{n}(t)$ +\end_inset + + that depends on a parameter +\begin_inset Formula $t$ +\end_inset + +, i.e., +\begin_inset Formula $T_{b}^{n}(t)$ +\end_inset + + follows a curve on the manifold, then it would be of interest to find the + velocity of a point +\begin_inset Formula $q^{n}(t)=T_{b}^{n}(t)p^{b}$ +\end_inset + + acted upon by +\begin_inset Formula $T_{b}^{n}(t)$ +\end_inset + +. + We can express the velocity of +\begin_inset Formula $q(t)$ +\end_inset + + in both the n-frame and b-frame: +\begin_inset Formula +\[ +\dot{q}^{n}=\dot{T}_{b}^{n}p^{b}=\dot{T}_{b}^{n}\left(T_{b}^{n}\right)^{-1}p^{n}\mbox{\,\,\,\,\ and\,\,\,\,}\dot{q}^{b}=\left(T_{b}^{n}\right)^{-1}\dot{q}^{n}=\left(T_{b}^{n}\right)^{-1}\dot{T}_{b}^{n}p^{b} +\] + +\end_inset + +Both the matrices +\begin_inset Formula $\xihat_{nb}^{n}\define\dot{T}_{b}^{n}\left(T_{b}^{n}\right)^{-1}$ +\end_inset + + and +\begin_inset Formula $\xihat_{nb}^{b}\define\left(T_{b}^{n}\right)^{-1}\dot{T}_{b}^{n}$ +\end_inset + + are skew-symmetric Lie algebra elements that describe the +\series bold +instantaneous velocity +\series default + +\begin_inset CommandInset citation +LatexCommand cite +after "page 51 for rotations, page 419 for SE(3)" +key "Murray94book" + +\end_inset + +. + We will revisit this for both rotations and rigid 3D transformations. +\end_layout + +\begin_layout Section +Differentials: Smooth Mapping between Lie Groups +\end_layout + +\begin_layout Subsection +Motivation and Definition +\end_layout + +\begin_layout Standard +The above shows how to compute the derivative of a function +\begin_inset Formula $f:G\rightarrow\Reals m$ +\end_inset + +. + However, what if the argument to +\begin_inset Formula $f$ +\end_inset + + is itself the result of a mapping between Lie groups? In other words, +\begin_inset Formula $f=g\circ\varphi$ +\end_inset + +, with +\begin_inset Formula $g:G\rightarrow\Reals m$ +\end_inset + + and where +\begin_inset Formula $\varphi:H\rightarrow G$ +\end_inset + + is a smooth mapping from the +\begin_inset Formula $n$ +\end_inset + +-dimensional Lie group +\begin_inset Formula $H$ +\end_inset + + to the +\begin_inset Formula $p$ +\end_inset + +-dimensional Lie group +\begin_inset Formula $G$ +\end_inset + +. + In this case, one would expect that we can arrive at +\begin_inset Formula $Df_{a}$ +\end_inset + + by composing linear maps, as follows: +\begin_inset Formula +\[ +f'(a)=(g\circ\varphi)'(a)=G_{\varphi(a)}\varphi'(a) +\] + +\end_inset + +where +\begin_inset Formula $\varphi'(a)$ +\end_inset + + is an +\begin_inset Formula $n\times p$ +\end_inset + + matrix that is the best linear approximation to the map +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\varphi:H\rightarrow G$ +\end_inset + +. + The corresponding linear map +\begin_inset Formula $D\varphi_{a}$ +\end_inset + + is called the +\family default +\series bold +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit +differential +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit +or +\series bold +pushforward +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + of +\begin_inset Formula $ $ +\end_inset + +the mapping +\begin_inset Formula $\varphi$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +. + +\end_layout + +\begin_layout Standard + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none +Because a rigorous definition will lead us too far astray, here we only + informally define the pushforward of +\begin_inset Formula $\varphi$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + + as the linear map +\begin_inset Formula $D\varphi_{a}:\Multi np$ +\end_inset + + such that +\begin_inset Formula $D\varphi_{a}\left(\xi\right)\define\varphi'(a)\xi$ +\end_inset + + and +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit + +\begin_inset Formula +\begin{equation} +\varphi\left(ae^{\xihat}\right)\approx\varphi\left(a\right)\exp\left(\widehat{\varphi'(a)\xi}\right)\label{eq:pushforward} +\end{equation} + +\end_inset + +with equality for +\begin_inset Formula $\xi\rightarrow0$ +\end_inset + +. + We call +\begin_inset Formula $\varphi'(a)$ +\end_inset + + the +\series bold +Jacobian matrix +\series default + of the map +\begin_inset Formula $\varphi$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +. + Below we show that even with this informal definition we can deduce the + pushforward in a number of useful cases. +\end_layout + +\begin_layout Subsection +Left Multiplication with a Constant +\end_layout + +\begin_layout Theorem +Suppose +\begin_inset Formula $G$ +\end_inset + + is an +\begin_inset Formula $n$ +\end_inset + +-dimensional Lie group, and +\begin_inset Formula $\varphi:G\rightarrow G$ +\end_inset + + is defined as +\begin_inset Formula $\varphi(g)=hg$ +\end_inset + +, with +\begin_inset Formula $h\in G$ +\end_inset + + a constant. + Then +\begin_inset Formula $D\varphi_{a}$ +\end_inset + + is the identity mapping and +\begin_inset Formula +\[ +\varphi'(a)=I_{n} +\] + +\end_inset + + +\end_layout + +\begin_layout Proof +Defining +\begin_inset Formula $y=D\varphi_{a}x$ +\end_inset + + as in +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:pushforward" + +\end_inset + +, we have +\begin_inset Formula +\begin{eqnarray*} +\varphi(a)e^{\yhat} & = & \varphi(ae^{\xhat})\\ +hae^{\yhat} & = & hae^{\xhat}\\ +y & = & x +\end{eqnarray*} + +\end_inset + + +\end_layout + +\begin_layout Subsection +Pushforward of the Inverse Mapping +\end_layout + +\begin_layout Standard +A well known property of Lie groups is the the fact that applying an incremental + change +\begin_inset Formula $\xihat$ +\end_inset + + in a different frame +\begin_inset Formula $g$ +\end_inset + + can be applied in a single step by applying the change +\begin_inset Formula $Ad_{g}\xihat$ +\end_inset + + in the original frame, +\begin_inset Formula +\begin{equation} +ge^{\xihat}g^{-1}=\exp\left(Ad_{g}\xihat\right)\label{eq:Adjoint2} +\end{equation} + +\end_inset + +where +\begin_inset Formula $Ad_{g}:\mathfrak{g}\rightarrow\mathfrak{g}$ +\end_inset + + is the +\series bold +adjoint representation +\series default +. + This comes in handy in the following: +\end_layout + +\begin_layout Theorem +Suppose that +\begin_inset Formula $\varphi:G\rightarrow G$ +\end_inset + + is defined as the mapping from an element +\begin_inset Formula $g$ +\end_inset + + to its +\series bold +inverse +\series default + +\begin_inset Formula $g^{-1}$ +\end_inset + +, i.e., +\begin_inset Formula $\varphi(g)=g^{-1}$ +\end_inset + +, then the pushforward +\begin_inset Formula $D\varphi_{a}$ +\end_inset + + satisfies +\begin_inset Formula +\begin{align} +\left(D\varphi_{a}x\right)\hat{} & =-Ad_{a}\xhat\label{eq:Dinverse} +\end{align} + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + In other words, and this is intuitive in hindsight, approximating the inverse + is accomplished by negation of +\begin_inset Formula $\xihat$ +\end_inset + +, along with an adjoint to make sure it is applied in the right frame. + +\begin_inset ERT +status open + +\begin_layout Plain Layout + + +\backslash +noindent +\end_layout + +\end_inset + + Note, however, that +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:Dinverse" + +\end_inset + + does not immediately yield a useful expression for the Jacobian matrix + +\begin_inset Formula $\varphi'(a)$ +\end_inset + +, but in many important cases this will turn out to be easy. + +\end_layout + +\begin_layout Proof +Defining +\begin_inset Formula $y=D\varphi_{a}x$ +\end_inset + + as in +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:pushforward" + +\end_inset + +, we have +\begin_inset Formula +\begin{eqnarray*} +\varphi(a)e^{\yhat} & = & \varphi(ae^{\xhat})\\ +a^{-1}e^{\yhat} & = & \left(ae^{\xhat}\right)^{-1}\\ +e^{\yhat} & = & -ae^{\xhat}a^{-1}\\ +\yhat & = & -\Ad a\xhat +\end{eqnarray*} + +\end_inset + + +\end_layout + +\begin_layout Example +For 3D rotations +\begin_inset Formula $R\in\SOthree$ +\end_inset + + we have +\begin_inset Formula +\[ +Ad_{g}(\hat{\omega})=R\hat{\omega}R^{T}=\Skew{R\omega} +\] + +\end_inset + +and hence the pushforward for the inverse mapping +\begin_inset Formula $\varphi(R)=R^{T}$ +\end_inset + + has the matrix +\begin_inset Formula $\varphi'(R)=-R$ +\end_inset + +. +\end_layout + +\begin_layout Subsection +Right Multiplication with a Constant +\end_layout + +\begin_layout Theorem +Suppose +\begin_inset Formula $\varphi:G\rightarrow G$ +\end_inset + + is defined as +\begin_inset Formula $\varphi(g)=gh$ +\end_inset + +, with +\begin_inset Formula $h\in G$ +\end_inset + + a constant. + Then +\begin_inset Formula $D\varphi_{a}$ +\end_inset + + satisfies +\begin_inset Formula +\[ +\left(D\varphi_{a}x\right)\hat{}=\Ad{h^{-1}}\xhat +\] + +\end_inset + + +\end_layout + +\begin_layout Proof +Defining +\begin_inset Formula $y=D\varphi_{a}x$ +\end_inset + + as in +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:pushforward" + +\end_inset + +, we have +\begin_inset Formula +\begin{align*} +\varphi(a)e^{\yhat} & =\varphi(ae^{\xhat})\\ +ahe & =ae^{\xhat}h\\ +e^{\yhat} & =h^{-1}e^{\xhat}h=\exp\left(\Ad{h^{-1}}\xhat\right)\\ +\yhat & =\Ad{h^{-1}}\xhat +\end{align*} + +\end_inset + + +\end_layout + +\begin_layout Example +In the case of 3D rotations, right multiplication with a constant rotation + +\begin_inset Formula $R$ +\end_inset + + is done through the mapping +\begin_inset Formula $\varphi(A)=AR$ +\end_inset + +, and satisfies +\begin_inset Formula +\[ +\Skew{D\varphi_{A}x}=\Ad{R^{T}}\Skew x +\] + +\end_inset + +For 3D rotations +\begin_inset Formula $R\in\SOthree$ +\end_inset + + we have +\begin_inset Formula +\[ +Ad_{R^{T}}(\hat{\omega})=R^{T}\hat{\omega}R=\Skew{R^{T}\omega} +\] + +\end_inset + +and hence the Jacobian matrix of +\begin_inset Formula $\varphi$ +\end_inset + + at +\begin_inset Formula $A$ +\end_inset + + is +\begin_inset Formula $\varphi'(A)=R^{T}$ +\end_inset + +. +\end_layout + +\begin_layout Subsection +Pushforward of Compose +\end_layout + +\begin_layout Theorem +If we define the mapping +\begin_inset Formula $\varphi:G\times G\rightarrow G$ +\end_inset + + as the product of two group elements +\begin_inset Formula $g,h\in G$ +\end_inset + +, i.e., +\begin_inset Formula $\varphi(g,h)=gh$ +\end_inset + +, then the pushforward will satisfy +\begin_inset Formula +\[ +D\varphi_{(a,b)}(x,y)=D_{1}\varphi_{(a,b)}x+D_{2}\varphi_{(a,b)}y +\] + +\end_inset + +with +\begin_inset Formula +\[ +\left(D_{1}\varphi_{(a,b)}x\right)\hat{}=\Ad{b^{-1}}\xhat\mbox{\;\ and\;}D_{2}\varphi_{(a,b)}y=y +\] + +\end_inset + + +\end_layout + +\begin_layout Proof +Looking at the first argument, the proof is very similar to right multiplication + with a constant +\begin_inset Formula $b$ +\end_inset + +. + Indeed, defining +\begin_inset Formula $y=D\varphi_{a}x$ +\end_inset + + as in +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:pushforward" + +\end_inset + +, we have +\begin_inset Formula +\begin{align} +\varphi(a,b)e^{\yhat} & =\varphi(ae^{\xhat},b)\nonumber \\ +abe^{\yhat} & =ae^{\xhat}b\nonumber \\ +e^{\yhat} & =b^{-1}e^{\xhat}b=\exp\left(\Ad{b^{-1}}\xhat\right)\nonumber \\ +\yhat & =\Ad{b^{-1}}\xhat\label{eq:Dcompose1} +\end{align} + +\end_inset + +In other words, to apply an incremental change +\begin_inset Formula $\xhat$ +\end_inset + + to +\begin_inset Formula $a$ +\end_inset + + we first need to undo +\begin_inset Formula $b$ +\end_inset + +, then apply +\begin_inset Formula $\xhat$ +\end_inset + +, and then apply +\begin_inset Formula $b$ +\end_inset + + again. + Using +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:Adjoint2" + +\end_inset + + this can be done in one step by simply applying +\begin_inset Formula $\Ad{b^{-1}}\xhat$ +\end_inset + +. + +\end_layout + +\begin_layout Proof +The second argument is quite a bit easier and simply yields the identity + mapping: +\begin_inset Formula +\begin{align} +\varphi(a,b)e^{\yhat} & =\varphi(a,be^{\xhat})\nonumber \\ +abe^{\yhat} & =abe^{\xhat}\nonumber \\ +y & =x\label{eq:Dcompose2} +\end{align} + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Note +status open + +\begin_layout Plain Layout +In summary, the Jacobian matrix of +\begin_inset Formula $\varphi(g,h)=gh$ +\end_inset + + at +\begin_inset Formula $(a,b)\in G\times G$ +\end_inset + + is given by +\begin_inset Formula +\[ +\varphi'(a,b)=? +\] + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Example +For 3D rotations +\begin_inset Formula $A,B\in\SOthree$ +\end_inset + + we have +\begin_inset Formula $\varphi(A,B)=AB$ +\end_inset + +, and +\begin_inset Formula $\Ad{B^{T}}\Skew{\omega}=\Skew{B^{T}\omega}$ +\end_inset + +, hence the Jacobian matrix +\begin_inset Formula $\varphi'(A,B)$ +\end_inset + + of composing two rotations is given by +\begin_inset Formula +\[ +\varphi'(A,B)=\left[\begin{array}{cc} +B^{T} & I_{3}\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Pushforward of Between +\end_layout + +\begin_layout Standard +Finally, let us find the pushforward of +\series bold +between +\series default +, defined as +\begin_inset Formula $\varphi(g,h)=g^{-1}h$ +\end_inset + +. + For the first argument we reason as: +\begin_inset Formula +\begin{align} +\varphi(g,h)e^{\yhat} & =\varphi(ge^{\xhat},h)\nonumber \\ +g^{-1}he^{\yhat} & =\left(ge^{\xhat}\right)^{-1}h=-e^{\xhat}g^{-1}h\nonumber \\ +e^{\yhat} & =-\left(h^{-1}g\right)e^{\xhat}\left(h^{-1}g\right)^{-1}=-\exp\Ad{\left(h^{-1}g\right)}\xhat\nonumber \\ +\yhat & =-\Ad{\left(h^{-1}g\right)}\xhat=-\Ad{\varphi\left(h,g\right)}\xhat\label{eq:Dbetween1} +\end{align} + +\end_inset + +The second argument yields the identity mapping. +\end_layout + +\begin_layout Example +For 3D rotations +\begin_inset Formula $A,B\in\SOthree$ +\end_inset + + we have +\begin_inset Formula $\varphi(A,B)=A^{T}B$ +\end_inset + +, and +\begin_inset Formula $\Ad{B^{T}A}\Skew{-\omega}=\Skew{-B^{T}A\omega}$ +\end_inset + +, hence the Jacobian matrix +\begin_inset Formula $\varphi'(A,B)$ +\end_inset + + of between is given by +\begin_inset Formula +\[ +\varphi'(A,B)=\left[\begin{array}{cc} +\left(-B^{T}A\right) & I_{3}\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Numerical PushForward +\end_layout + +\begin_layout Standard +Let's examine +\begin_inset Formula +\[ +f\left(g\right)e^{\yhat}=f\left(ge^{\xhat}\right) +\] + +\end_inset + +and multiply with +\begin_inset Formula $f(g)^{-1}$ +\end_inset + + on both sides: +\begin_inset Formula +\[ +e^{\yhat}=f\left(g\right)^{-1}f\left(ge^{\xhat}\right) +\] + +\end_inset + +We then take the log (which in our case returns +\begin_inset Formula $y$ +\end_inset + +, not +\begin_inset Formula $\yhat$ +\end_inset + +): +\begin_inset Formula +\[ +y(x)=\log\left[f\left(g\right)^{-1}f\left(ge^{\xhat}\right)\right] +\] + +\end_inset + +Let us look at +\begin_inset Formula $x=0$ +\end_inset + +, and perturb in direction +\begin_inset Formula $i$ +\end_inset + +, +\begin_inset Formula $e_{i}=[0,0,1,0,0]$ +\end_inset + +. + Then take derivative, +\begin_inset Formula +\[ +\deriv{y(d)}d\define\lim_{d\rightarrow0}\frac{y(d)-y(0)}{d}=\lim_{d\rightarrow0}\frac{1}{d}\log\left[f\left(g\right)^{-1}f\left(ge^{\widehat{de_{i}}}\right)\right] +\] + +\end_inset + +which is the basis for a numerical derivative scheme. +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +Not understood yet: Let us also look at a chain rule. + If we know the behavior at the origin +\begin_inset Formula $I$ +\end_inset + +, we can extrapolate +\begin_inset Formula +\[ +f(ge^{\xhat})=f(ge^{\xhat}g^{-1}g)=f(e^{\Ad g\xhat}g) +\] + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Subsection +Derivative of the Exponential and Logarithm Map +\end_layout + +\begin_layout Theorem +\begin_inset CommandInset label +LatexCommand label +name "D-exp" + +\end_inset + +The derivative of the function +\begin_inset Formula $f:\Reals n\rightarrow G$ +\end_inset + + that applies the wedge operator followed by the exponential map, i.e., +\begin_inset Formula $f(\xi)=\exp\xihat$ +\end_inset + +, is the identity map for +\begin_inset Formula $\xi=0$ +\end_inset + +. +\end_layout + +\begin_layout Proof +For +\begin_inset Formula $\xi=0$ +\end_inset + +, we have +\begin_inset Formula +\begin{eqnarray*} +f(\xi)e^{\yhat} & = & f(\xi+x)\\ +f(0)e^{\yhat} & = & f(0+x)\\ +e^{\yhat} & = & e^{\xhat} +\end{eqnarray*} + +\end_inset + + +\end_layout + +\begin_layout Corollary +The derivative of the inverse +\begin_inset Formula $f^{-1}$ +\end_inset + + is the identity as well, i.e., for +\begin_inset Formula $T=e$ +\end_inset + +, the identity element in +\begin_inset Formula $G$ +\end_inset + +. +\end_layout + +\begin_layout Standard +For +\begin_inset Formula $\xi\neq0$ +\end_inset + +, things are not simple, see . + +\begin_inset Flex URL +status collapsed + +\begin_layout Plain Layout + +http://deltaepsilons.wordpress.com/2009/11/06/helgasons-formula-for-the-differenti +al-of-the-exponential/ +\end_layout + +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +General Manifolds +\end_layout + +\begin_layout Subsection +Retractions +\end_layout + +\begin_layout Standard +\begin_inset FormulaMacro +\newcommand{\retract}{\mathcal{R}} +{\mathcal{R}} +\end_inset + + +\end_layout + +\begin_layout Standard +General manifolds that are not Lie groups do not have an exponential map, + but can still be handled by defining a +\series bold +retraction +\series default + +\begin_inset Formula $\retract:\Man\times\Reals n\rightarrow\Man$ +\end_inset + +, such that +\begin_inset Formula +\[ +a\oplus\xi\define\retract_{a}\left(\xi\right) +\] + +\end_inset + +A retraction +\begin_inset CommandInset citation +LatexCommand cite +key "Absil07book" + +\end_inset + + is required to be tangent to geodesics on the manifold +\begin_inset Formula $\Man$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +. + We can define many retractions for a manifold +\begin_inset Formula $\Man$ +\end_inset + +, even for those with more structure. + For the vector space +\begin_inset Formula $\Reals n$ +\end_inset + + the retraction is just vector addition, and for Lie groups the obvious + retraction is simply the exponential map, i.e., +\begin_inset Formula $\retract_{a}(\xi)=a\cdot\exp\xihat$ +\end_inset + +. + However, one can choose other, possibly computationally attractive retractions, + as long as around a they agree with the geodesic induced by the exponential + map, i.e., +\begin_inset Formula +\[ +\lim_{\xi\rightarrow0}\frac{\left|a\cdot\exp\xihat-\retract_{a}\left(\xi\right)\right|}{\left|\xi\right|}=0 +\] + +\end_inset + + +\end_layout + +\begin_layout Example +For +\begin_inset Formula $\SEthree$ +\end_inset + +, instead of using the true exponential map it is computationally more efficient + to define the retraction, which uses a first order approximation of the + translation update +\begin_inset Formula +\[ +\retract_{T}\left(\left[\begin{array}{c} +\omega\\ +v +\end{array}\right]\right)=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +e^{\Skew{\omega}} & v\\ +0 & 1 +\end{array}\right]=\left[\begin{array}{cc} +Re^{\Skew{\omega}} & t+Rv\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Derivatives +\end_layout + +\begin_layout Standard +Equipped with a retraction, then, we can generalize the notion of a derivative + for functions +\begin_inset Formula $f$ +\end_inset + + from general a manifold +\begin_inset Formula $\Man$ +\end_inset + + to +\begin_inset Formula $\Reals m$ +\end_inset + +: +\end_layout + +\begin_layout Definition +We define a function +\begin_inset Formula $f:\Man\rightarrow\Reals m$ +\end_inset + + to be +\series bold +differentiable +\series default + at +\begin_inset Formula $a\in\Man$ +\end_inset + + if there exists a matrix +\begin_inset Formula $f'(a)$ +\end_inset + + such that +\begin_inset Formula +\[ +\lim_{\xi\rightarrow0}\frac{\left|f(a)+f'(a)\xi-f\left(\retract_{a}(\xi)\right)\right|}{\left|\xi\right|}=0 +\] + +\end_inset + +with +\begin_inset Formula $\xi\in\Reals n$ +\end_inset + + for an +\begin_inset Formula $n$ +\end_inset + +-dimensional manifold, and +\begin_inset Formula $\retract_{a}:\Reals n\rightarrow\Man$ +\end_inset + + a retraction +\begin_inset Formula $\retract$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +. + If +\begin_inset Formula $f$ +\end_inset + + is differentiable, then +\begin_inset Formula $f'(a)$ +\end_inset + + is called the +\series bold +Jacobian matrix +\series default + of +\begin_inset Formula $f$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +, and the linear transformation +\begin_inset Formula $Df_{a}:\xi\mapsto f'(a)\xi$ +\end_inset + + is called the +\series bold +derivative +\series default + of +\begin_inset Formula $f$ +\end_inset + + at +\begin_inset Formula $a$ +\end_inset + +. +\end_layout + +\begin_layout Definition +For manifolds that are also Lie groups, the derivative of any function +\begin_inset Formula $f:G\rightarrow\Reals m$ +\end_inset + + will agree no matter what retraction +\begin_inset Formula $\retract$ +\end_inset + + is used. +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Part +Practice +\end_layout + +\begin_layout Standard +Below we apply the results derived in the theory part to the geometric objects + we use in GTSAM. + Above we preferred the modern notation +\begin_inset Formula $D_{1}f$ +\end_inset + + for the partial derivative. + Below (because this was written earlier) we use the more classical notation + +\begin_inset Formula +\[ +\deriv{f(x,y)}x +\] + +\end_inset + +In addition, for Lie groups we will abuse the notation and take +\begin_inset Formula +\[ +\at{\deriv{\varphi(g)}{\xi}}a +\] + +\end_inset + +to be the Jacobian matrix +\begin_inset Formula $\varphi'($ +\end_inset + +a) of the mapping +\begin_inset Formula $\varphi$ +\end_inset + + at +\begin_inset Formula $a\in G$ +\end_inset + +, associated with the pushforward +\begin_inset Formula $D\varphi_{a}$ +\end_inset + +. +\end_layout + +\begin_layout Section +SLAM Example +\end_layout + +\begin_layout Standard +Let us examine a visual SLAM example. + We have 2D measurements +\begin_inset Formula $z_{ij}$ +\end_inset + +, where each measurement is predicted by +\begin_inset Formula +\[ +z_{ij}=h(T_{i},p_{j})=\pi(T_{i}^{-1}p_{j}) +\] + +\end_inset + +where +\begin_inset Formula $T_{i}$ +\end_inset + + is the 3D pose of the +\begin_inset Formula $i^{th}$ +\end_inset + + camera, +\begin_inset Formula $p_{j}$ +\end_inset + + is the location of the +\begin_inset Formula $j^{th}$ +\end_inset + + point, and +\begin_inset Formula $\pi:(x,y,z)\mapsto(x/z,y/z)$ +\end_inset + + is the camera projection function from Example +\begin_inset CommandInset ref +LatexCommand ref +reference "ex:projection" + +\end_inset + +. +\end_layout + +\begin_layout Section +BetweenFactor +\end_layout + +\begin_layout Standard +BetweenFactor is often used to summarize +\end_layout + +\begin_layout Standard +Theorem +\begin_inset CommandInset ref +LatexCommand ref +reference "D-exp" + +\end_inset + + about the derivative of the exponential map +\begin_inset Formula $f:\xi\mapsto\exp\xihat$ +\end_inset + + being identity only at +\begin_inset Formula $\xi=0$ +\end_inset + + has implications for GTSAM. + Given two elements +\begin_inset Formula $T_{1}$ +\end_inset + + and +\begin_inset Formula $T_{2}$ +\end_inset + +, BetweenFactor evaluates +\begin_inset Formula +\[ +g(T_{1},T_{2};Z)=f^{-1}\left(\mathop{between}(Z,\mathop{between}(T_{1},T_{2})\right)=f^{-1}\left(Z^{-1}\left(T_{1}^{-1}T_{2}\right)\right) +\] + +\end_inset + +but because it is assumed that +\begin_inset Formula $Z\approx T_{1}^{-1}T_{2}$ +\end_inset + +, and hence we have +\begin_inset Formula $Z^{-1}T_{1}^{-1}T_{2}\approx e$ +\end_inset + + and the derivative should be good there. + Note that the derivative of +\emph on +between +\emph default + is identity in its second argument. +\end_layout + +\begin_layout Section +Point3 +\end_layout + +\begin_layout Standard +A cross product +\begin_inset Formula $a\times b$ +\end_inset + + can be written as a matrix multiplication +\begin_inset Formula +\[ +a\times b=\Skew ab +\] + +\end_inset + +where +\begin_inset Formula $\Skew a$ +\end_inset + + is a skew-symmetric matrix defined as +\begin_inset Formula +\[ +\Skew{x,y,z}=\left[\begin{array}{ccc} +0 & -z & y\\ +z & 0 & -x\\ +-y & x & 0 +\end{array}\right] +\] + +\end_inset + +We also have +\begin_inset Formula +\[ +a^{T}\Skew b=-(\Skew ba)^{T}=-(a\times b)^{T} +\] + +\end_inset + +The derivative of a cross product +\begin_inset Formula +\begin{equation} +\frac{\partial(a\times b)}{\partial a}=\Skew{-b}\label{eq:Dcross1} +\end{equation} + +\end_inset + + +\begin_inset Formula +\begin{equation} +\frac{\partial(a\times b)}{\partial b}=\Skew a\label{eq:Dcross2} +\end{equation} + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +2D Rotations +\end_layout + +\begin_layout Subsection +Rot2 in GTSAM +\end_layout + +\begin_layout Standard +A rotation is stored as +\begin_inset Formula $(\cos\theta,\sin\theta)$ +\end_inset + +. + An incremental rotation is applied using the trigonometric sum rule: +\begin_inset Formula +\[ +\cos\theta'=\cos\theta\cos\delta-\sin\theta\sin\delta +\] + +\end_inset + + +\begin_inset Formula +\[ +\sin\theta'=\sin\theta\cos\delta+\cos\theta\sin\delta +\] + +\end_inset + +where +\begin_inset Formula $\delta$ +\end_inset + + is an incremental rotation angle. +\end_layout + +\begin_layout Subsection +Derivatives of Actions +\end_layout + +\begin_layout Standard +In the case of +\begin_inset Formula $\SOtwo$ +\end_inset + + the vector space is +\begin_inset Formula $\Rtwo$ +\end_inset + +, and the group action +\begin_inset Formula $f(R,p)$ +\end_inset + + corresponds to rotating the 2D point +\begin_inset Formula $p$ +\end_inset + + +\begin_inset Formula +\[ +f(R,p)=Rp +\] + +\end_inset + +According to Theorem +\begin_inset CommandInset ref +LatexCommand ref +reference "th:Action" + +\end_inset + +, the Jacobian matrix of +\begin_inset Formula $f$ +\end_inset + + is given by +\begin_inset Formula +\[ +f'(R,p)=\left[\begin{array}{cc} +RH(p) & R\end{array}\right] +\] + +\end_inset + +with +\begin_inset Formula $H:\Reals 2\rightarrow\Reals{2\times2}$ +\end_inset + + a linear mapping that depends on +\begin_inset Formula $p$ +\end_inset + +. + In the case of +\begin_inset Formula $\SOtwo$ +\end_inset + +, we can find +\begin_inset Formula $H(p)$ +\end_inset + + by equating (as in Equation +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:Hp" + +\end_inset + +): +\begin_inset Formula +\[ +\skew wp=\left[\begin{array}{cc} +0 & -\omega\\ +\omega & 0 +\end{array}\right]\left[\begin{array}{c} +x\\ +y +\end{array}\right]=\left[\begin{array}{c} +-y\\ +x +\end{array}\right]\omega=H(p)\omega +\] + +\end_inset + +Note that +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula +\[ +H(p)=\left[\begin{array}{c} +-y\\ +x +\end{array}\right]=\left[\begin{array}{cc} +0 & -1\\ +1 & 0 +\end{array}\right]\left[\begin{array}{c} +x\\ +y +\end{array}\right]=R_{\pi/2}p +\] + +\end_inset + +and since 2D rotations commute, we also have, with +\begin_inset Formula $q=Rp$ +\end_inset + + +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\uuline default +\uwave default +\noun default +\color inherit +: +\begin_inset Formula +\[ +f'(R,p)=\left[\begin{array}{cc} +R\left(R_{\pi/2}p\right) & R\end{array}\right]=\left[\begin{array}{cc} +R_{\pi/2}q & R\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Pushforwards of Mappings +\end_layout + +\begin_layout Standard +Since +\begin_inset Formula $\Ad R\skew{\omega}=\skew{\omega}$ +\end_inset + +, we have the derivative of +\series bold +inverse +\series default +, +\begin_inset Formula +\[ +\frac{\partial R^{T}}{\partial\omega}=-\Ad R=-1\mbox{ } +\] + +\end_inset + + +\series bold +compose, +\series default + +\begin_inset Formula +\[ +\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{1}}=\Ad{R_{2}^{T}}=1\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}}=1 +\] + +\end_inset + +and +\series bold +between: +\series default + +\begin_inset Formula +\[ +\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{1}}=-\Ad{R_{2}^{T}R_{1}}=-1\mbox{ and }\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{2}}=1 +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +2D Rigid Transformations +\end_layout + +\begin_layout Subsection +The derivatives of Actions +\end_layout + +\begin_layout Standard +The action of +\begin_inset Formula $\SEtwo$ +\end_inset + + on 2D points is done by embedding the points in +\begin_inset Formula $\mathbb{R}^{3}$ +\end_inset + + by using homogeneous coordinates +\begin_inset Formula +\[ +f(T,p)=\hat{q}=\left[\begin{array}{c} +q\\ +1 +\end{array}\right]=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{c} +p\\ +1 +\end{array}\right]=T\hat{p} +\] + +\end_inset + +To find the derivative, we write the quantity +\begin_inset Formula $\xihat\hat{p}$ +\end_inset + + as the product of the +\begin_inset Formula $3\times3$ +\end_inset + + matrix +\begin_inset Formula $H(p)$ +\end_inset + + with +\begin_inset Formula $\xi$ +\end_inset + +: +\begin_inset Formula +\begin{equation} +\xihat\hat{p}=\left[\begin{array}{cc} +\skew{\omega} & v\\ +0 & 0 +\end{array}\right]\left[\begin{array}{c} +p\\ +1 +\end{array}\right]=\left[\begin{array}{c} +\skew{\omega}p+v\\ +0 +\end{array}\right]=\left[\begin{array}{cc} +I_{2} & R_{\pi/2}p\\ +0 & 0 +\end{array}\right]\left[\begin{array}{c} +v\\ +\omega +\end{array}\right]=H(p)\xi\label{eq:HpSE2} +\end{equation} + +\end_inset + +Hence, by Theorem +\begin_inset CommandInset ref +LatexCommand ref +reference "th:Action" + +\end_inset + + we have +\begin_inset Formula +\begin{equation} +\deriv{\left(T\hat{p}\right)}{\xi}=TH(p)=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +I_{2} & R_{\pi/2}p\\ +0 & 0 +\end{array}\right]=\left[\begin{array}{cc} +R & RR_{\pi/2}p\\ +0 & 0 +\end{array}\right]=\left[\begin{array}{cc} +R & R_{\pi/2}q\\ +0 & 0 +\end{array}\right]\label{eq:SE2Action} +\end{equation} + +\end_inset + +Note that, looking only at the top rows of +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:HpSE2" + +\end_inset + + and +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:SE2Action" + +\end_inset + +, we can recognize the quantity +\begin_inset Formula $\skew{\omega}p+v=v+\omega\left(R_{\pi/2}p\right)$ +\end_inset + + as the velocity of +\begin_inset Formula $p$ +\end_inset + + in +\begin_inset Formula $\Rtwo$ +\end_inset + +, and +\begin_inset Formula $\left[\begin{array}{cc} +R & R_{\pi/2}q\end{array}\right]$ +\end_inset + + is the derivative of the action on +\begin_inset Formula $\Rtwo$ +\end_inset + +. + +\end_layout + +\begin_layout Standard +The derivative of the inverse action +\begin_inset Formula $g(T,p)=T^{-1}\hat{p}$ +\end_inset + + is given by Theorem +\begin_inset CommandInset ref +LatexCommand ref +reference "Th:InverseAction" + +\end_inset + + specialized to +\begin_inset Formula $\SEtwo$ +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\deriv{\left(T^{-1}\hat{p}\right)}{\xi}=-H(T^{-1}p)=\left[\begin{array}{cc} +-I_{2} & -R_{\pi/2}\left(T^{-1}p\right)\\ +0 & 0 +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Pushforwards of Mappings +\end_layout + +\begin_layout Standard +We can just define all derivatives in terms of the adjoint map, which in + the case of +\begin_inset Formula $\SEtwo$ +\end_inset + +, in twist coordinates, is the linear mapping +\begin_inset Formula +\[ +\Ad T\xi=\left[\begin{array}{cc} +R & -R_{\pi/2}t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{c} +v\\ +\omega +\end{array}\right] +\] + +\end_inset + +and we have +\begin_inset Formula +\begin{eqnarray*} +\frac{\partial T^{^{-1}}}{\partial\xi} & = & -\Ad T +\end{eqnarray*} + +\end_inset + + +\begin_inset Formula +\begin{eqnarray*} +\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{1}} & = & \Ad{T_{2}^{^{-1}}}\mbox{ and }\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{2}}=I_{3} +\end{eqnarray*} + +\end_inset + + +\begin_inset Formula +\begin{eqnarray*} +\frac{\partial\left(T_{1}^{-1}T_{2}\right)}{\partial\xi_{1}} & = & -\Ad{T_{2}^{^{-1}}T_{1}}=-\Ad{between(T_{2},T_{1})}\mbox{ and }\frac{\partial\left(T_{1}^{-1}T_{2}\right)}{\partial\xi_{2}}=I_{3} +\end{eqnarray*} + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +3D Rotations +\end_layout + +\begin_layout Subsection +Derivatives of Actions +\end_layout + +\begin_layout Standard +In the case of +\begin_inset Formula $\SOthree$ +\end_inset + + the vector space is +\begin_inset Formula $\Rthree$ +\end_inset + +, and the group action +\begin_inset Formula $f(R,p)$ +\end_inset + + corresponds to rotating a point +\begin_inset Formula +\[ +q=f(R,p)=Rp +\] + +\end_inset + +To calculate +\begin_inset Formula $H(p)$ +\end_inset + + for use in Theorem +\begin_inset CommandInset ref +LatexCommand eqref +reference "th:Action" + +\end_inset + + we make use of +\begin_inset Formula +\[ +\Skew{\omega}p=\omega\times p=-p\times\omega=\Skew{-p}\omega +\] + +\end_inset + +so +\begin_inset Formula $H(p)\define\Skew{-p}$ +\end_inset + +. + Hence, the final derivative of an action in its first argument is +\begin_inset Formula +\[ +\deriv{\left(Rp\right)}{\omega}=RH(p)=-R\Skew p +\] + +\end_inset + +Likewise, according to Theorem +\begin_inset CommandInset ref +LatexCommand ref +reference "Th:InverseAction" + +\end_inset + +, the derivative of the inverse action is given by +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\deriv{\left(R^{T}p\right)}{\omega}=-H(R^{T}p)=\Skew{R^{T}p} +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +\begin_inset CommandInset label +LatexCommand label +name "sub:3DAngularVelocities" + +\end_inset + +Instantaneous Velocity +\end_layout + +\begin_layout Standard +For 3D rotations +\begin_inset Formula $R_{b}^{n}$ +\end_inset + + from a body frame +\begin_inset Formula $b$ +\end_inset + + to a navigation frame +\begin_inset Formula $n$ +\end_inset + + we have the spatial angular velocity +\begin_inset Formula $\omega_{nb}^{n}$ +\end_inset + + measured in the navigation frame, +\begin_inset Formula +\[ +\Skew{\omega_{nb}^{n}}\define\dot{R}_{b}^{n}\left(R_{b}^{n}\right)^{T}=\dot{R}_{b}^{n}R_{n}^{b} +\] + +\end_inset + +and the body angular velocity +\begin_inset Formula $\omega_{nb}^{b}$ +\end_inset + + measured in the body frame: +\begin_inset Formula +\[ +\Skew{\omega_{nb}^{b}}\define\left(R_{b}^{n}\right)^{T}\dot{R}_{b}^{n}=R_{n}^{b}\dot{R}_{b}^{n} +\] + +\end_inset + +These quantities can be used to derive the velocity of a point +\begin_inset Formula $p$ +\end_inset + +, and we choose between spatial or body angular velocity depending on the + frame in which we choose to represent +\begin_inset Formula $p$ +\end_inset + +: +\begin_inset Formula +\[ +v^{n}=\Skew{\omega_{nb}^{n}}p^{n}=\omega_{nb}^{n}\times p^{n} +\] + +\end_inset + + +\begin_inset Formula +\[ +v^{b}=\Skew{\omega_{nb}^{b}}p^{b}=\omega_{nb}^{b}\times p^{b} +\] + +\end_inset + +We can transform these skew-symmetric matrices from navigation to body frame + by conjugating, +\begin_inset Formula +\[ +\Skew{\omega_{nb}^{b}}=R_{n}^{b}\Skew{\omega_{nb}^{n}}R_{b}^{n} +\] + +\end_inset + +but because the adjoint representation satisfies +\begin_inset Formula +\[ +Ad_{R}\Skew{\omega}\define R\Skew{\omega}R^{T}=\Skew{R\omega} +\] + +\end_inset + +we can even more easily transform between spatial and body angular velocities + as 3-vectors: +\begin_inset Formula +\[ +\omega_{nb}^{b}=R_{n}^{b}\omega_{nb}^{n} +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Pushforwards of Mappings +\end_layout + +\begin_layout Standard +For +\begin_inset Formula $\SOthree$ +\end_inset + + we have +\begin_inset Formula $\Ad R\Skew{\omega}=\Skew{R\omega}$ +\end_inset + + and, in terms of angular velocities: +\begin_inset Formula $\Ad R\omega=R\omega$ +\end_inset + +. + Hence, the Jacobian matrix of the +\series bold +inverse +\series default + mapping is (see Equation +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:Dinverse" + +\end_inset + +) +\begin_inset Formula +\[ +\frac{\partial R^{T}}{\partial\omega}=-\Ad R=-R +\] + +\end_inset + +for +\series bold +compose +\series default + we have (Equations +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:Dcompose1" + +\end_inset + + and +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:Dcompose2" + +\end_inset + +): +\begin_inset Formula +\[ +\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{1}}=R_{2}^{T}\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}}=I_{3} +\] + +\end_inset + +and +\series bold +between +\series default + (Equation +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:Dbetween1" + +\end_inset + +): +\begin_inset Formula +\[ +\frac{\partial\left(R_{1}^{T}R_{2}\right)}{\partial\omega_{1}}=-R_{2}^{T}R_{1}=-between(R_{2},R_{1})\mbox{ and }\frac{\partial\left(R_{1}R_{2}\right)}{\partial\omega_{2}}=I_{3} +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Retractions +\end_layout + +\begin_layout Standard +Absil +\begin_inset CommandInset citation +LatexCommand cite +after "page 58" +key "Absil07book" + +\end_inset + + discusses two possible retractions for +\begin_inset Formula $\SOthree$ +\end_inset + + based on the QR decomposition or the polar decomposition of the matrix + +\begin_inset Formula $R\Skew{\omega}$ +\end_inset + +, but they are expensive. + Another retraction is based on the Cayley transform +\begin_inset Formula $\mathcal{C}:\sothree\rightarrow\SOthree$ +\end_inset + +, a mapping from the skew-symmetric matrices to rotation matrices: +\begin_inset Formula +\[ +Q=\mathcal{C}(\Omega)=(I-\Omega)(I+\Omega)^{-1} +\] + +\end_inset + +Interestingly, the inverse Cayley transform +\begin_inset Formula $\mathcal{C}^{-1}:\SOthree\rightarrow\sothree$ +\end_inset + + has the same form: +\begin_inset Formula +\[ +\Omega=\mathcal{C}^{-1}(Q)=(I-Q)(I+Q)^{-1} +\] + +\end_inset + +The retraction needs a factor +\begin_inset Formula $-\frac{1}{2}$ +\end_inset + + however, to make it locally align with a geodesic: +\begin_inset Formula +\[ +R'=\retract_{R}(\omega)=R\mathcal{C}(-\frac{1}{2}\Skew{\omega}) +\] + +\end_inset + +Note that given +\begin_inset Formula $\omega=(x,y,z)$ +\end_inset + + this has the closed-form expression below +\begin_inset Formula +\[ +\frac{1}{4+x^{2}+y^{2}+z^{2}}\left[\begin{array}{ccc} +4+x^{2}-y^{2}-z^{2} & 2xy-4z & 2xz+4y\\ +2xy+4z & 4-x^{2}+y^{2}-z^{2} & 2yz-4x\\ +2xz-4y & 2yz+4x & 4-x^{2}-y^{2}+z^{2} +\end{array}\right] +\] + +\end_inset + + +\begin_inset Formula +\[ +=\frac{1}{4+x^{2}+y^{2}+z^{2}}\left\{ 4(I+\Skew{\omega})+\left[\begin{array}{ccc} +x^{2}-y^{2}-z^{2} & 2xy & 2xz\\ +2xy & -x^{2}+y^{2}-z^{2} & 2yz\\ +2xz & 2yz & -x^{2}-y^{2}+z^{2} +\end{array}\right]\right\} +\] + +\end_inset + +so it can be seen to be a second-order correction on +\begin_inset Formula $(I+\Skew{\omega})$ +\end_inset + +. + The corresponding approximation to the logarithmic map is: +\begin_inset Formula +\[ +\Skew{\omega}=\retract_{R}^{-1}(R')=-2\mathcal{C}^{-1}\left(R^{T}R'\right) +\] + +\end_inset + + +\end_layout + +\begin_layout Section +3D Rigid Transformations +\end_layout + +\begin_layout Subsection +The derivatives of 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\\ +1 +\end{array}\right]=f(T,p)=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{c} +p\\ +1 +\end{array}\right]=T\hat{p} +\] + +\end_inset + +The quantity +\begin_inset Formula $\xihat\hat{p}$ +\end_inset + + corresponds to a velocity in +\begin_inset Formula $\mathbb{R}^{4}$ +\end_inset + + (in the local +\begin_inset Formula $T$ +\end_inset + + frame), and equating it to +\begin_inset Formula $H(p)\xi$ +\end_inset + + as in Equation +\begin_inset CommandInset ref +LatexCommand ref +reference "eq:Hp" + +\end_inset + + yields the +\begin_inset Formula $4\times6$ +\end_inset + + matrix +\begin_inset Formula $H(p)$ +\end_inset + + +\begin_inset Foot +status collapsed + +\begin_layout Plain Layout +\begin_inset Formula $H(p)$ +\end_inset + + can also be obtained by taking the +\begin_inset Formula $j^{th}$ +\end_inset + + column of each of the 6 generators to multiply with components of +\begin_inset Formula $\hat{p}$ +\end_inset + + +\end_layout + +\end_inset + +: +\begin_inset Formula +\[ +\xihat\hat{p}=\left[\begin{array}{cc} +\Skew{\omega} & v\\ +0 & 0 +\end{array}\right]\left[\begin{array}{c} +p\\ +1 +\end{array}\right]=\left[\begin{array}{c} +\omega\times p+v\\ +0 +\end{array}\right]=\left[\begin{array}{cc} +\Skew{-p} & I_{3}\\ +0 & 0 +\end{array}\right]\left[\begin{array}{c} +\omega\\ +v +\end{array}\right]=H(p)\xi +\] + +\end_inset + +Note how velocities are analogous to points at infinity in projective geometry: + they correspond to free vectors indicating a direction and magnitude of + change. + According to Theorem +\begin_inset CommandInset ref +LatexCommand ref +reference "th:Action" + +\end_inset + +, the derivative of the group action is then +\begin_inset Formula +\[ +\deriv{\left(T\hat{p}\right)}{\xi}=TH(p)=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +\Skew{-p} & I_{3}\\ +0 & 0 +\end{array}\right]=\left[\begin{array}{cc} +R\Skew{-p} & R\\ +0 & 0 +\end{array}\right] +\] + +\end_inset + + +\begin_inset Formula +\[ +\deriv{\left(T\hat{p}\right)}{\hat{p}}=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + +in homogenous coordinates. + In +\begin_inset Formula $\Rthree$ +\end_inset + + this becomes +\begin_inset Formula $R\left[\begin{array}{cc} +-\Skew p & I_{3}\end{array}\right]$ +\end_inset + +. +\end_layout + +\begin_layout Standard +The derivative of the inverse action +\begin_inset Formula $T^{-1}p$ +\end_inset + + is given by Theorem +\begin_inset CommandInset ref +LatexCommand ref +reference "Th:InverseAction" + +\end_inset + +: +\end_layout + +\begin_layout Standard + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\noun off +\color none +\begin_inset Formula +\[ +\deriv{\left(T^{-1}\hat{p}\right)}{\xi}=-H\left(T^{-1}\hat{p}\right)=\left[\begin{array}{cc} +\Skew{T^{-1}\hat{p}} & -I_{3}\end{array}\right] +\] + +\end_inset + + +\begin_inset Formula +\[ +\deriv{\left(T^{-1}\hat{p}\right)}{\hat{p}}=\left[\begin{array}{cc} +R^{T} & -R^{T}t\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Example +Let us examine a visual SLAM example. + We have 2D measurements +\begin_inset Formula $z_{ij}$ +\end_inset + +, where each measurement is predicted by +\begin_inset Formula +\[ +z_{ij}=h(T_{i},p_{j})=\pi(T_{i}^{-1}p_{j})=\pi(q) +\] + +\end_inset + +where +\begin_inset Formula $T_{i}$ +\end_inset + + is the 3D pose of the +\begin_inset Formula $i^{th}$ +\end_inset + + camera, +\begin_inset Formula $p_{j}$ +\end_inset + + is the location of the +\begin_inset Formula $j^{th}$ +\end_inset + + point, +\begin_inset Formula $q=(x',y',z')=T^{-1}p$ +\end_inset + + is the point in camera coordinates, and +\begin_inset Formula $\pi:(x,y,z)\mapsto(x/z,y/z)$ +\end_inset + + is the camera projection function from Example +\begin_inset CommandInset ref +LatexCommand ref +reference "ex:projection" + +\end_inset + +. + By the chain rule, we then have +\begin_inset Formula +\[ +\deriv{h(T,p)}{\xi}=\deriv{\pi(q)}q\deriv{(T^{-1}p)}{\xi}=\frac{1}{z'}\left[\begin{array}{ccc} +1 & 0 & -x'/z'\\ +0 & 1 & -y'/z' +\end{array}\right]\left[\begin{array}{cc} +\Skew q & -I_{3}\end{array}\right]=\left[\begin{array}{cc} +\pi'(q)\Skew q & -\pi'(q)\end{array}\right] +\] + +\end_inset + + +\begin_inset Formula +\[ +\deriv{h(T,p)}p=\pi'(q)R^{T} +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Instantaneous Velocity +\end_layout + +\begin_layout Standard +For rigid 3D transformations +\begin_inset Formula $T_{b}^{n}$ +\end_inset + + from a body frame +\begin_inset Formula $b$ +\end_inset + + to a navigation frame +\begin_inset Formula $n$ +\end_inset + + we have the instantaneous spatial twist +\begin_inset Formula $\xi_{nb}^{n}$ +\end_inset + + measured in the navigation frame, +\begin_inset Formula +\[ +\hat{\xi}_{nb}^{n}\define\dot{T}_{b}^{n}\left(T_{b}^{n}\right)^{-1} +\] + +\end_inset + +and the instantaneous body twist +\begin_inset Formula $\xi_{nb}^{b}$ +\end_inset + + measured in the body frame: +\begin_inset Formula +\[ +\hat{\xi}_{nb}^{b}\define\left(T_{b}^{n}\right)^{T}\dot{T}_{b}^{n} +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection +Pushforwards of Mappings +\end_layout + +\begin_layout Standard +As we can express the Adjoint representation in terms of twist coordinates, + we have +\begin_inset Formula +\[ +\left[\begin{array}{c} +\omega'\\ +v' +\end{array}\right]=\left[\begin{array}{cc} +R & 0\\ +\Skew tR & R +\end{array}\right]\left[\begin{array}{c} +\omega\\ +v +\end{array}\right] +\] + +\end_inset + +Hence, as with +\begin_inset Formula $\SOthree$ +\end_inset + +, we are now in a position to simply posit the derivative of +\series bold +inverse +\series default +, +\begin_inset Formula +\[ +\frac{\partial T^{-1}}{\partial\xi}=\Ad T=-\left[\begin{array}{cc} +R & 0\\ +\Skew tR & R +\end{array}\right] +\] + +\end_inset + + +\series bold +compose +\series default + in its first argument, +\begin_inset Formula +\[ +\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{1}}=\Ad{T_{2}^{-1}} +\] + +\end_inset + + in its second argument, +\begin_inset Formula +\[ +\frac{\partial\left(T_{1}T_{2}\right)}{\partial\xi_{2}}=I_{6} +\] + +\end_inset + + +\series bold +between +\series default + in its first argument, +\begin_inset Formula +\[ +\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}}=\Ad{T_{2}^{^{-1}}T_{1}} +\] + +\end_inset + +and in its second argument, +\begin_inset Formula +\begin{eqnarray*} +\frac{\partial\left(T_{1}^{^{-1}}T_{2}\right)}{\partial\xi_{1}} & = & I_{6} +\end{eqnarray*} + +\end_inset + + +\end_layout + +\begin_layout Subsection +Retractions +\end_layout + +\begin_layout Standard +For +\begin_inset Formula $\SEthree$ +\end_inset + +, instead of using the true exponential map it is computationally more efficient + to design other retractions. + A first-order approximation to the exponential map does not quite cut it, + as it yields a +\begin_inset Formula $4\times4$ +\end_inset + + matrix which is not in +\begin_inset Formula $\SEthree$ +\end_inset + +: +\begin_inset Formula +\begin{eqnarray*} +T\exp\xihat & \approx & T(I+\xihat)\\ + & = & T\left(I_{4}+\left[\begin{array}{cc} +\Skew{\omega} & v\\ +0 & 0 +\end{array}\right]\right)\\ + & = & \left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +I_{3}+\Skew{\omega} & v\\ +0 & 1 +\end{array}\right]\\ + & = & \left[\begin{array}{cc} +R\left(I_{3}+\Skew{\omega}\right) & t+Rv\\ +0 & 1 +\end{array}\right] +\end{eqnarray*} + +\end_inset + +However, we can make it into a retraction by using any retraction defined + for +\begin_inset Formula $\SOthree$ +\end_inset + +, including, as below, using the exponential map +\begin_inset Formula $Re^{\Skew{\omega}}$ +\end_inset + +: +\begin_inset Formula +\[ +\retract_{T}\left(\left[\begin{array}{c} +\omega\\ +v +\end{array}\right]\right)=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +e^{\Skew{\omega}} & v\\ +0 & 1 +\end{array}\right]=\left[\begin{array}{cc} +Re^{\Skew{\omega}} & t+Rv\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + +Similarly, for a second order approximation we have +\begin_inset Formula +\begin{eqnarray*} +T\exp\xihat & \approx & T(I+\xihat+\frac{\xihat^{2}}{2})\\ + & = & T\left(I_{4}+\left[\begin{array}{cc} +\Skew{\omega} & v\\ +0 & 0 +\end{array}\right]+\frac{1}{2}\left[\begin{array}{cc} +\Skew{\omega} & v\\ +0 & 0 +\end{array}\right]\left[\begin{array}{cc} +\Skew{\omega} & v\\ +0 & 0 +\end{array}\right]\right)\\ + & = & \left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left(\left[\begin{array}{cc} +I_{3}+\Skew{\omega}+\frac{1}{2}\Skew{\omega}^{2} & v+\frac{1}{2}\Skew{\omega}v\\ +0 & 1 +\end{array}\right]\right)\\ + & = & \left[\begin{array}{cc} +R\left(I_{3}+\Skew{\omega}+\frac{1}{2}\Skew{\omega}^{2}\right) & t+R\left[v+\left(\omega\times v\right)/2\right]\\ +0 & 1 +\end{array}\right] +\end{eqnarray*} + +\end_inset + +inspiring the retraction +\begin_inset Formula +\[ +\retract_{T}\left(\left[\begin{array}{c} +\omega\\ +v +\end{array}\right]\right)=\left[\begin{array}{cc} +R & t\\ +0 & 1 +\end{array}\right]\left[\begin{array}{cc} +e^{\Skew{\omega}} & v+\left(\omega\times v\right)/2\\ +0 & 1 +\end{array}\right]=\left[\begin{array}{cc} +Re^{\Skew{\omega}} & t+R\left[v+\left(\omega\times v\right)/2\right]\\ +0 & 1 +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Newpage pagebreak +\end_inset + + +\end_layout + +\begin_layout Section +2D Line Segments (Ocaml) +\end_layout + +\begin_layout Standard +The error between an infinite line +\begin_inset Formula $(a,b,c)$ +\end_inset + + and a 2D line segment +\begin_inset Formula $((x1,y1),(x2,y2))$ +\end_inset + + is defined in Line3.ml. +\end_layout + +\begin_layout Section +Line3vd (Ocaml) +\end_layout + +\begin_layout Standard +One representation of a line is through 2 vectors +\begin_inset Formula $(v,d)$ +\end_inset + +, where +\begin_inset Formula $v$ +\end_inset + + is the direction and the vector +\begin_inset Formula $d$ +\end_inset + + points from the orgin to the closest point on the line. +\end_layout + +\begin_layout Standard +In this representation, transforming a 3D line from a world coordinate frame + to a camera at +\begin_inset Formula $(R_{w}^{c},t^{w})$ +\end_inset + + is done by +\begin_inset Formula +\[ +v^{c}=R_{w}^{c}v^{w} +\] + +\end_inset + + +\begin_inset Formula +\[ +d^{c}=R_{w}^{c}\left(d^{w}+(t^{w}v^{w})v^{w}-t^{w}\right) +\] + +\end_inset + + +\end_layout + +\begin_layout Section +Line3 (Ocaml) +\end_layout + +\begin_layout Standard +For 3D lines, we use a parameterization due to C.J. + Taylor, using a rotation matrix +\begin_inset Formula $R$ +\end_inset + + and 2 scalars +\begin_inset Formula $a$ +\end_inset + + and +\begin_inset Formula $b$ +\end_inset + +. + The line direction +\begin_inset Formula $v$ +\end_inset + + is simply the Z-axis of the rotated frame, i.e., +\begin_inset Formula $v=R_{3}$ +\end_inset + +, while the vector +\begin_inset Formula $d$ +\end_inset + + is given by +\begin_inset Formula $d=aR_{1}+bR_{2}$ +\end_inset + +. +\end_layout + +\begin_layout Standard +Now, we will +\emph on +not +\emph default + use the incremental rotation scheme we used for rotations: because the + matrix R translates from the line coordinate frame to the world frame, + we need to apply the incremental rotation on the right-side: +\begin_inset Formula +\[ +R'=R(I+\Omega) +\] + +\end_inset + +Projecting a line to 2D can be done easily, as both +\begin_inset Formula $v$ +\end_inset + + and +\begin_inset Formula $d$ +\end_inset + + are also the 2D homogenous coordinates of two points on the projected line, + and hence we have +\begin_inset Formula +\begin{eqnarray*} +l & = & v\times d\\ + & = & R_{3}\times\left(aR_{1}+bR_{2}\right)\\ + & = & a\left(R_{3}\times R_{1}\right)+b\left(R_{3}\times R_{2}\right)\\ + & = & aR_{2}-bR_{1} +\end{eqnarray*} + +\end_inset + +This can be written as a rotation of a point, +\begin_inset Formula +\[ +l=R\left(\begin{array}{c} +-b\\ +a\\ +0 +\end{array}\right) +\] + +\end_inset + +but because the incremental rotation is now done on the right, we need to + figure out the derivatives again: +\begin_inset Formula +\begin{equation} +\frac{\partial(R(I+\Omega)x)}{\partial\omega}=\frac{\partial(R\Omega x)}{\partial\omega}=R\frac{\partial(\Omega x)}{\partial\omega}=R\Skew{-x}\label{eq:rotateRight} +\end{equation} + +\end_inset + +and hence the derivative of the projection +\begin_inset Formula $l$ +\end_inset + + with respect to the rotation matrix +\begin_inset Formula $R$ +\end_inset + +of the 3D line is +\begin_inset Formula +\begin{equation} +\frac{\partial(l)}{\partial\omega}=R\Skew{\left(\begin{array}{c} +b\\ +-a\\ +0 +\end{array}\right)}=\left[\begin{array}{ccc} +aR_{3} & bR_{3} & -(aR_{1}+bR_{2})\end{array}\right] +\end{equation} + +\end_inset + +or the +\begin_inset Formula $a,b$ +\end_inset + + scalars: +\begin_inset Formula +\[ +\frac{\partial(l)}{\partial a}=R_{2} +\] + +\end_inset + + +\begin_inset Formula +\[ +\frac{\partial(l)}{\partial b}=-R_{1} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +Transforming a 3D line +\begin_inset Formula $(R,(a,b))$ +\end_inset + + from a world coordinate frame to a camera frame +\begin_inset Formula $(R_{w}^{c},t^{w})$ +\end_inset + + is done by +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +R'=R_{w}^{c}R +\] + +\end_inset + + +\begin_inset Formula +\[ +a'=a-R_{1}^{T}t^{w} +\] + +\end_inset + + +\begin_inset Formula +\[ +b'=b-R_{2}^{T}t^{w} +\] + +\end_inset + +Again, we need to redo the derivatives, as R is incremented from the right. + The first argument is incremented from the left, but the result is incremented + on the right: +\begin_inset Formula +\begin{eqnarray*} +R'(I+\Omega')=(AB)(I+\Omega') & = & (I+\Skew{S\omega})AB\\ +I+\Omega' & = & (AB)^{T}(I+\Skew{S\omega})(AB)\\ +\Omega' & = & R'^{T}\Skew{S\omega}R'\\ +\Omega' & = & \Skew{R'^{T}S\omega}\\ +\omega' & = & R'^{T}S\omega +\end{eqnarray*} + +\end_inset + +For the second argument +\begin_inset Formula $R$ +\end_inset + + we now simply have: +\begin_inset Formula +\begin{eqnarray*} +AB(I+\Omega') & = & AB(I+\Omega)\\ +\Omega' & = & \Omega\\ +\omega' & = & \omega +\end{eqnarray*} + +\end_inset + +The scalar derivatives can be found by realizing that +\begin_inset Formula +\[ +\left(\begin{array}{c} +a'\\ +b'\\ +... +\end{array}\right)=\left(\begin{array}{c} +a\\ +b\\ +0 +\end{array}\right)-R^{T}t^{w} +\] + +\end_inset + +where we don't care about the third row. + Hence +\begin_inset Formula +\[ +\frac{\partial(\left(R(I+\Omega_{2})\right)^{T}t^{w})}{\partial\omega}=-\frac{\partial(\Omega_{2}R^{T}t^{w})}{\partial\omega}=-\Skew{R^{T}t^{w}}=\left[\begin{array}{ccc} +0 & R_{3}^{T}t^{w} & -R_{2}^{T}t^{w}\\ +-R_{3}^{T}t^{w} & 0 & R_{1}^{T}t^{w}\\ +... & ... & 0 +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Section + +\series bold +Aligning 3D Scans +\end_layout + +\begin_layout Standard +Below is the explanaition underlying Pose3.align, i.e. + aligning two point clouds using SVD. + Inspired but modified from CVOnline... +\end_layout + +\begin_layout Standard + +\emph on +Our +\emph default + model is +\begin_inset Formula +\[ +p^{c}=R\left(p^{w}-t\right) +\] + +\end_inset + +i.e., +\begin_inset Formula $R$ +\end_inset + + is from camera to world, and +\begin_inset Formula $t$ +\end_inset + + is the camera location in world coordinates. + The objective function is +\begin_inset Formula +\begin{equation} +\frac{1}{2}\sum\left(p^{c}-R(p^{w}-t)\right)^{2}=\frac{1}{2}\sum\left(p^{c}-Rp^{w}+Rt\right)^{2}=\frac{1}{2}\sum\left(p^{c}-Rp^{w}-t'\right)^{2}\label{eq:J} +\end{equation} + +\end_inset + +where +\begin_inset Formula $t'=-Rt$ +\end_inset + + is the location of the origin in the camera frame. + Taking the derivative with respect to +\begin_inset Formula $t'$ +\end_inset + + and setting to zero we have +\begin_inset Formula +\[ +\sum\left(p^{c}-Rp^{w}-t'\right)=0 +\] + +\end_inset + +or +\begin_inset Formula +\begin{equation} +t'=\frac{1}{n}\sum\left(p^{c}-Rp^{w}\right)=\bar{p}^{c}-R\bar{p}^{w}\label{eq:t} +\end{equation} + +\end_inset + +here +\begin_inset Formula $\bar{p}^{c}$ +\end_inset + + and +\begin_inset Formula $\bar{p}^{w}$ +\end_inset + + are the point cloud centroids. + Substituting back into +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:J" + +\end_inset + +, we get +\begin_inset Formula +\[ +\frac{1}{2}\sum\left(p^{c}-R(p^{w}-t)\right)^{2}=\frac{1}{2}\sum\left(\left(p^{c}-\bar{p}^{c}\right)-R\left(p^{w}-\bar{p}^{w}\right)\right)^{2}=\frac{1}{2}\sum\left(\hat{p}^{c}-R\hat{p}^{w}\right)^{2} +\] + +\end_inset + +Now, to minimize the above it suffices to maximize (see CVOnline) +\begin_inset Formula +\[ +\mathop{trace}\left(R^{T}C\right) +\] + +\end_inset + +where +\begin_inset Formula $C=\sum\hat{p}^{c}\left(\hat{p}^{w}\right)^{T}$ +\end_inset + + is the correlation matrix. + Intuitively, the cloud of points is rotated to align with the principal + axes. + This can be achieved by SVD decomposition on +\begin_inset Formula $C$ +\end_inset + + +\begin_inset Formula +\[ +C=USV^{T} +\] + +\end_inset + +and setting +\begin_inset Formula +\[ +R=UV^{T} +\] + +\end_inset + +Clearly, from +\begin_inset CommandInset ref +LatexCommand eqref +reference "eq:t" + +\end_inset + + we then also recover the optimal +\begin_inset Formula $t$ +\end_inset + + as +\begin_inset Formula +\[ +t=\bar{p}^{w}-R^{T}\bar{p}^{c} +\] + +\end_inset + + +\end_layout + +\begin_layout Section* +Appendix +\end_layout + +\begin_layout Subsection* +Differentiation Rules +\end_layout + +\begin_layout Standard +Spivak +\begin_inset CommandInset citation +LatexCommand cite +key "Spivak65book" + +\end_inset + + also notes some multivariate derivative rules defined component-wise, but + they are not that useful in practice: +\end_layout + +\begin_layout Itemize +Since +\begin_inset Formula $f:\Multi nm$ +\end_inset + + is defined in terms of +\begin_inset Formula $m$ +\end_inset + + component functions +\begin_inset Formula $f^{i}$ +\end_inset + +, then +\begin_inset Formula $f$ +\end_inset + + is differentiable at +\begin_inset Formula $a$ +\end_inset + + iff each +\begin_inset Formula $f^{i}$ +\end_inset + + is, and the Jacobian matrix +\begin_inset Formula $F_{a}$ +\end_inset + + is the +\begin_inset Formula $m\times n$ +\end_inset + + matrix whose +\begin_inset Formula $i^{th}$ +\end_inset + + row is +\begin_inset Formula $\left(f^{i}\right)'(a)$ +\end_inset + +: +\begin_inset Formula +\[ +F_{a}\define f'(a)=\left[\begin{array}{c} +\left(f^{1}\right)'(a)\\ +\vdots\\ +\left(f^{m}\right)'(a) +\end{array}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Itemize +Scalar differentiation rules: if +\begin_inset Formula $f,g:\OneD n$ +\end_inset + + are differentiable at +\begin_inset Formula $a$ +\end_inset + +, then +\begin_inset Formula +\[ +(f+g)'(a)=F_{a}+G_{a} +\] + +\end_inset + + +\begin_inset Formula +\[ +(f\cdot g)'(a)=g(a)F_{a}+f(a)G_{a} +\] + +\end_inset + + +\begin_inset Formula +\[ +(f/g)'(a)=\frac{1}{g(a)^{2}}\left[g(a)F_{a}-f(a)G_{a}\right] +\] + +\end_inset + + +\end_layout + +\begin_layout Subsection* +Tangent Spaces and the Tangent Bundle +\end_layout + +\begin_layout Standard +The following is adapted from Appendix A in +\begin_inset CommandInset citation +LatexCommand cite +key "Murray94book" + +\end_inset + +. +\end_layout + +\begin_layout Standard +The +\series bold +tangent space +\series default + +\begin_inset Formula $T_{p}M$ +\end_inset + + of a manifold +\begin_inset Formula $M$ +\end_inset + + at a point +\begin_inset Formula $p\in M$ +\end_inset + + is the vector space of +\series bold +tangent vectors +\series default + at +\begin_inset Formula $p$ +\end_inset + +. + The +\series bold +tangent bundle +\series default + +\begin_inset Formula $TM$ +\end_inset + + is the set of all tangent vectors +\begin_inset Formula +\[ +TM\define\bigcup_{p\in M}T_{p}M +\] + +\end_inset + +A +\series bold +vector field +\series default + +\begin_inset Formula $X:M\rightarrow TM$ +\end_inset + + assigns a single tangent vector +\begin_inset Formula $x\in T_{p}M$ +\end_inset + + to each point +\begin_inset Formula $p$ +\end_inset + +. +\end_layout + +\begin_layout Standard +If +\begin_inset Formula $F:M\rightarrow N$ +\end_inset + + is a smooth map from a manifold +\begin_inset Formula $M$ +\end_inset + + to a manifold +\begin_inset Formula $N$ +\end_inset + +, then we can define the +\series bold + tangent map +\series default + of +\begin_inset Formula $F$ +\end_inset + + at +\begin_inset Formula $p$ +\end_inset + + as the linear map +\begin_inset Formula $F_{*p}:T_{p}M\rightarrow T_{F(p)}N$ +\end_inset + + that maps tangent vectors in +\begin_inset Formula $T_{p}M$ +\end_inset + + at +\begin_inset Formula $p$ +\end_inset + + to tangent vectors in +\begin_inset Formula $T_{F(p)}N$ +\end_inset + + at the image +\begin_inset Formula $F(p)$ +\end_inset + +. +\end_layout + +\begin_layout Subsection* +Homomorphisms +\end_layout + +\begin_layout Standard +The following +\emph on +might be +\emph default + relevant +\begin_inset CommandInset citation +LatexCommand cite +after "page 45" +key "Hall00book" + +\end_inset + +: suppose that +\begin_inset Formula $\Phi:G\rightarrow H$ +\end_inset + + is a mapping (Lie group homomorphism). + Then there exists a unique linear map +\begin_inset Formula $\phi:\gg\rightarrow\mathfrak{h}$ +\end_inset + + +\begin_inset Formula +\[ +\phi(\xhat)\define\lim_{t\rightarrow0}\frac{d}{dt}\Phi\left(e^{t\xhat}\right) +\] + +\end_inset + +such that +\end_layout + +\begin_layout Enumerate +\begin_inset Formula $\Phi\left(e^{\xhat}\right)=e^{\phi\left(\xhat\right)}$ +\end_inset + + +\end_layout + +\begin_layout Enumerate +\begin_inset Formula $\phi\left(T\xhat T^{-1}\right)=\Phi(T)\phi(\xhat)\Phi(T^{-1})$ +\end_inset + + +\end_layout + +\begin_layout Enumerate +\begin_inset Formula $\phi\left([\xhat,\yhat]\right)=\left[\phi(\xhat),\phi(\yhat)\right]$ +\end_inset + + +\end_layout + +\begin_layout Standard +In other words, the map +\begin_inset Formula $\phi$ +\end_inset + + is the derivative of +\begin_inset Formula $\Phi$ +\end_inset + + at the identity. + As an example, suppose +\begin_inset Formula $\Phi(g)=g^{-1}$ +\end_inset + +, then the corresponding derivative +\emph on +at the identity +\emph default +is +\begin_inset Formula +\[ +\phi(\xhat)\define\lim_{t\rightarrow0}\frac{d}{dt}\left(e^{t\xhat}\right)^{-1}=\lim_{t\rightarrow0}\frac{d}{dt}e^{-t\xhat}=-\xhat\lim_{t\rightarrow0}e^{-t\xhat}=-\xhat +\] + +\end_inset + +In general it suffices to compute +\begin_inset Formula $\phi$ +\end_inset + + for a basis of +\begin_inset Formula $\gg$ +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +Undercooked: What if we want the derivative of +\begin_inset Formula $\Phi$ +\end_inset + + at some other element +\begin_inset Formula $g$ +\end_inset + +? In other words, if we apply +\begin_inset Formula $\Phi$ +\end_inset + + at +\begin_inset Formula $g$ +\end_inset + + incremented by some Lie algebra element +\begin_inset Formula $e^{\xhat}$ +\end_inset + +, then we are looking for a +\begin_inset Formula $\yhat\in\gg$ +\end_inset + + will yield the same result: +\begin_inset Formula +\[ +\Phi\left(g\right)\lim_{t\rightarrow0}\frac{d}{dt}e^{t\yhat}=\lim_{t\rightarrow0}\frac{d}{dt}\Phi\left(ge^{t\xhat}\right) +\] + +\end_inset + + +\begin_inset Formula +\[ +\lim_{t\rightarrow0}\frac{d}{dt}e^{t\yhat}=\Phi\left(g\right)^{-1}\lim_{t\rightarrow0}\frac{d}{dt}\Phi\left(ge^{t\xhat}\right) +\] + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Note Note +status collapsed + +\begin_layout Plain Layout +Let us define two mappings +\begin_inset Formula +\[ +\Phi_{1}(A)=AB\mbox{ and }\Phi_{2}(B)=AB +\] + +\end_inset + +Then +\begin_inset Formula +\[ +\phi_{1}(\xhat)=\lim_{t\rightarrow0}\frac{d}{dt}\Phi_{1}\left(e^{t\xhat}B\right)= +\] + +\end_inset + + +\end_layout + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset CommandInset bibtex +LatexCommand bibtex +bibfiles "/Users/dellaert/papers/refs" +options "plain" + +\end_inset + + +\end_layout + +\end_body +\end_document diff --git a/doc/math.pdf b/doc/math.pdf new file mode 100644 index 000000000..ee50857ce Binary files /dev/null and b/doc/math.pdf differ diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index c5c2efc66..cec4301dc 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -63,14 +63,8 @@ int main(void) { // 4. Single Step Optimization using Levenberg-Marquardt // Note: Although there are many options in IterativeOptimizationParameters, - // the SimpleSPCGSolver doesn't actually use all of them at this moment. - // More detail in the next release. - LevenbergMarquardtParams param; - param.linearSolverType = SuccessiveLinearizationParams::CG; - param.iterativeParams = boost::make_shared(); - - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param); - Values result = optimizer.optimize(); + Values result = graph.optimizeSPCG(initialEstimate); + result.print("\nFinal result:\n"); cout << "final error = " << graph.error(result) << endl; return 0 ; diff --git a/examples/VisualSLAMwISAM2Example.cpp b/examples/VisualISAMExample.cpp similarity index 95% rename from examples/VisualSLAMwISAM2Example.cpp rename to examples/VisualISAMExample.cpp index 1ec43136f..dca24dba5 100644 --- a/examples/VisualSLAMwISAM2Example.cpp +++ b/examples/VisualISAMExample.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file VisualSLAMwISAM2Example.cpp + * @file VisualISAMExample.cpp * @brief An ISAM example for synthesis sequence, single camera * @author Duy-Nguyen Ta */ @@ -19,7 +19,7 @@ #include #include #include -#include "VisualSLAMExampleData.h" +#include "VisualSLAMData.h" using namespace std; using namespace gtsam; @@ -72,8 +72,8 @@ int main(int argc, char* argv[]) { initials.insert(X(1), pose0Init*odoMeasurement); // Initial values for the landmarks, simulated with Gaussian noise - for (size_t j=0; jsample())); + for (size_t j=0; jsample())); // Update ISAM the first time and obtain the current estimate isam.update(newFactors, initials); diff --git a/examples/VisualSLAMExampleData.h b/examples/VisualSLAMData.h similarity index 73% rename from examples/VisualSLAMExampleData.h rename to examples/VisualSLAMData.h index f0ee4d2b5..155a137d8 100644 --- a/examples/VisualSLAMExampleData.h +++ b/examples/VisualSLAMData.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file VisualSLAMSimulatedData.cpp - * @brief Generate ground-truth simulated data for VisualSLAM examples (SFM and ISAM2) + * @file VisualSLAMData.cpp + * @brief Generate ground-truth simulated data for VisualSLAM examples * @author Duy-Nguyen Ta */ @@ -23,7 +23,7 @@ /* ************************************************************************* */ /** - * Simulated data for the example: + * Simulated data for the visual SLAM examples: * - 8 Landmarks: (10,10,10) (-10,10,10) (-10,-10,10) (10,-10,10) * (10,10,-10) (-10,10,-10) (-10,-10,-10) (10,-10,-10) * - n 90-deg-FoV cameras with the same calibration parameters: @@ -40,7 +40,7 @@ struct VisualSLAMExampleData { gtsam::shared_ptrK sK; // camera calibration parameters std::vector poses; // ground-truth camera poses gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM) - std::vector landmarks; // ground-truth landmarks + std::vector points; // ground-truth landmarks std::map > z; // 2D measurements of landmarks in each camera frame gtsam::SharedDiagonal noiseZ; // measurement noise (noiseModel::Isotropic::Sigma(2, 5.0f)); gtsam::SharedDiagonal noiseX; // noise for camera poses @@ -49,14 +49,14 @@ struct VisualSLAMExampleData { static const VisualSLAMExampleData generate() { VisualSLAMExampleData data; // Landmarks (ground truth) - data.landmarks.push_back(gtsam::Point3(10.0,10.0,10.0)); - data.landmarks.push_back(gtsam::Point3(-10.0,10.0,10.0)); - data.landmarks.push_back(gtsam::Point3(-10.0,-10.0,10.0)); - data.landmarks.push_back(gtsam::Point3(10.0,-10.0,10.0)); - data.landmarks.push_back(gtsam::Point3(10.0,10.0,-10.0)); - data.landmarks.push_back(gtsam::Point3(-10.0,10.0,-10.0)); - data.landmarks.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); - data.landmarks.push_back(gtsam::Point3(10.0,-10.0,-10.0)); + data.points.push_back(gtsam::Point3(10.0,10.0,10.0)); + data.points.push_back(gtsam::Point3(-10.0,10.0,10.0)); + data.points.push_back(gtsam::Point3(-10.0,-10.0,10.0)); + data.points.push_back(gtsam::Point3(10.0,-10.0,10.0)); + data.points.push_back(gtsam::Point3(10.0,10.0,-10.0)); + data.points.push_back(gtsam::Point3(-10.0,10.0,-10.0)); + data.points.push_back(gtsam::Point3(-10.0,-10.0,-10.0)); + data.points.push_back(gtsam::Point3(10.0,-10.0,-10.0)); // Camera calibration parameters data.sK = gtsam::shared_ptrK(new gtsam::Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); @@ -65,8 +65,7 @@ struct VisualSLAMExampleData { int n = 8; double theta = 0.0; double r = 30.0; - for (int i=0; isample())); + data.z[i].push_back(camera.project(data.points[j]) + gtsam::Point2(data.noiseZ->sample())); } } - data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.01, 0.01, 0.01, 0.1, 0.1, 0.1)); + data.noiseX = gtsam::sharedSigmas(gtsam::Vector_(6, 0.001, 0.001, 0.001, 0.1, 0.1, 0.1)); data.noiseL = gtsam::sharedSigma(3, 0.1); return data; diff --git a/examples/VisualSLAMforSFMExample.cpp b/examples/VisualSLAMExample.cpp similarity index 84% rename from examples/VisualSLAMforSFMExample.cpp rename to examples/VisualSLAMExample.cpp index 17f457908..90391bf90 100644 --- a/examples/VisualSLAMforSFMExample.cpp +++ b/examples/VisualSLAMExample.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file VisualSLAMforSFMExample.cpp + * @file VisualSLAMExample.cpp * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset * @author Duy-Nguyen Ta */ @@ -19,7 +19,7 @@ #include #include #include -#include "VisualSLAMExampleData.h" +#include "VisualSLAMData.h" using namespace std; using namespace gtsam; @@ -39,18 +39,19 @@ int main(int argc, char* argv[]) { /* 2. Add factors to the graph */ // 2a. Measurement factors for (size_t i=0; isample())); - for (size_t j=0; jsample())); + for (size_t j=0; jsample())); initial.print("Intial Estimates: "); /* 4. Optimize the graph and print results */ diff --git a/examples/matlab/LocalizationExample.m b/examples/matlab/LocalizationExample.m index ae66a131c..dacd6d972 100644 --- a/examples/matlab/LocalizationExample.m +++ b/examples/matlab/LocalizationExample.m @@ -43,26 +43,13 @@ initialEstimate.print(sprintf('\nInitial estimate:\n ')); result = graph.optimize(initialEstimate); result.print(sprintf('\nFinal result:\n ')); -%% Query the marginals -marginals = graph.marginals(result); -P{1}=marginals.marginalCovariance(1); -P{2}=marginals.marginalCovariance(2); -P{3}=marginals.marginalCovariance(3); - -%% Plot Trajectory -figure(1) -clf -X=[];Y=[]; -for i=1:3 - pose_i = result.pose(i); - X=[X;pose_i.x]; - Y=[Y;pose_i.y]; -end -plot(X,Y,'b*-'); - %% Plot Covariance Ellipses -hold on -for i=1:3 - pose_i = result.pose(i); - covarianceEllipse([pose_i.x;pose_i.y],P{i},'g') +figure(1);clf; +plot(result.xs(),result.ys(),'k*-'); hold on +marginals = graph.marginals(result); +for i=1:result.size() + pose_i = result.pose(i); + P{i}=marginals.marginalCovariance(i); + plotPose2(pose_i,'g',P{i}) end +axis equal diff --git a/examples/matlab/Pose2SLAMExample.m b/examples/matlab/Pose2SLAMExample.m index 5641d2ac3..a63c2fd80 100644 --- a/examples/matlab/Pose2SLAMExample.m +++ b/examples/matlab/Pose2SLAMExample.m @@ -54,3 +54,16 @@ initialEstimate.print(sprintf('\nInitial estimate:\n')); %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd result = graph.optimize(initialEstimate); result.print(sprintf('\nFinal result:\n')); + +%% Plot Covariance Ellipses +figure(1);clf; +plot(result.xs(),result.ys(),'k*-'); hold on +plot([result.pose(5).x;result.pose(2).x],[result.pose(5).y;result.pose(2).y],'r-'); +marginals = graph.marginals(result); +for i=1:result.size() + pose_i = result.pose(i); + P{i}=marginals.marginalCovariance(i); + fprintf(1,'%.5f %.5f %.5f\n',P{i}) + plotPose2(pose_i,'g',P{i}) +end +axis equal diff --git a/examples/matlab/Pose2SLAMExample_graph.m b/examples/matlab/Pose2SLAMExample_graph.m index 142c9b36d..5564ef30e 100644 --- a/examples/matlab/Pose2SLAMExample_graph.m +++ b/examples/matlab/Pose2SLAMExample_graph.m @@ -34,6 +34,6 @@ marginals = graph.marginals(result); for i=1:result.size()-1 pose_i = result.pose(i); P{i}=marginals.marginalCovariance(i); - covarianceEllipse([pose_i.x;pose_i.y],P{i},'b') + plotPose2(pose_i,'b',P{i}) end fprintf(1,'%.5f %.5f %.5f\n',P{99}) \ No newline at end of file diff --git a/examples/matlab/Pose2SLAMwSPCG.m b/examples/matlab/Pose2SLAMwSPCG.m new file mode 100644 index 000000000..f9c0537be --- /dev/null +++ b/examples/matlab/Pose2SLAMwSPCG.m @@ -0,0 +1,54 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief Simple 2D robotics example using the SimpleSPCGSolver +% @author Yong-Dian Jian +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Assumptions +% - All values are axis aligned +% - Robot poses are facing along the X axis (horizontal, to the right in images) +% - We have full odometry for measurements +% - The robot is on a grid, moving 2 meters each step + +%% Create graph container and add factors to it +graph = pose2SLAMGraph; + +%% Add prior +% gaussian for prior +priorMean = gtsamPose2(0.0, 0.0, 0.0); % prior at origin +priorNoise = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); +graph.addPrior(1, priorMean, priorNoise); % add directly to graph + +%% Add odometry +% general noisemodel for odometry +odometryNoise = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); +graph.addOdometry(1, 2, gtsamPose2(2.0, 0.0, 0.0 ), odometryNoise); +graph.addOdometry(2, 3, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addOdometry(3, 4, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); +graph.addOdometry(4, 5, gtsamPose2(2.0, 0.0, pi/2), odometryNoise); + +%% Add pose constraint +model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); +graph.addConstraint(5, 2, gtsamPose2(2.0, 0.0, pi/2), model); + +% print +graph.print(sprintf('\nFactor graph:\n')); + +%% Initialize to noisy points +initialEstimate = pose2SLAMValues; +initialEstimate.insertPose(1, gtsamPose2(0.5, 0.0, 0.2 )); +initialEstimate.insertPose(2, gtsamPose2(2.3, 0.1,-0.2 )); +initialEstimate.insertPose(3, gtsamPose2(4.1, 0.1, pi/2)); +initialEstimate.insertPose(4, gtsamPose2(4.0, 2.0, pi )); +initialEstimate.insertPose(5, gtsamPose2(2.1, 2.1,-pi/2)); +initialEstimate.print(sprintf('\nInitial estimate:\n')); + +%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +result = graph.optimizeSPCG(initialEstimate); +result.print(sprintf('\nFinal result:\n')); \ No newline at end of file diff --git a/examples/matlab/Pose3SLAMExample_circle.m b/examples/matlab/Pose3SLAMExample_circle.m index d1676626c..1c8e481ee 100644 --- a/examples/matlab/Pose3SLAMExample_circle.m +++ b/examples/matlab/Pose3SLAMExample_circle.m @@ -39,11 +39,11 @@ initial.insertPose(5, hexagon.pose(5).retract(s*randn(6,1))); %% Plot Initial Estimate figure(1);clf -plot3(initial.xs(),initial.ys(),initial.zs(),'g-*'); axis equal +plot3(initial.xs(),initial.ys(),initial.zs(),'g-*'); %% optimize result = fg.optimize(initial); %% Show Result -hold on; plot3(result.xs(),result.ys(),result.zs(),'b-*') +hold on; plot3DTrajectory(result,'b-*', true, 0.3); axis equal; result.print(sprintf('\nFinal result:\n')); diff --git a/examples/matlab/Pose3SLAMExample_graph.m b/examples/matlab/Pose3SLAMExample_graph.m index b1f0afb1a..115e4341c 100644 --- a/examples/matlab/Pose3SLAMExample_graph.m +++ b/examples/matlab/Pose3SLAMExample_graph.m @@ -16,18 +16,17 @@ N = 2500; filename = '../Data/sphere2500.txt'; %% Initialize graph, initial estimate, and odometry noise -model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); [graph,initial]=load3D(filename,model,true,N); -first = initial.pose(0); -graph.addHardConstraint(0, first); +model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]); %% Plot Initial Estimate figure(1);clf +first = initial.pose(0); plot3(first.x(),first.y(),first.z(),'r*'); hold on plot3DTrajectory(initial,'g-',false); -%% Read again, now all constraints -[graph,discard]=load3D(filename,model,false,N); +%% Read again, now with all constraints, and optimize +graph = load3D(filename,model,false,N); graph.addHardConstraint(0, first); -result = graph.optimize(initial); % start from old result +result = graph.optimize(initial); plot3DTrajectory(result,'r-',false); axis equal; diff --git a/examples/matlab/VisualSLAMExample.m b/examples/matlab/VisualSLAMExample.m new file mode 100644 index 000000000..c5b0f7969 --- /dev/null +++ b/examples/matlab/VisualSLAMExample.m @@ -0,0 +1,119 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% GTSAM Copyright 2010, Georgia Tech Research Corporation, +% Atlanta, Georgia 30332-0415 +% All Rights Reserved +% Authors: Frank Dellaert, et al. (see THANKS for the full author list) +% +% See LICENSE for the license information +% +% @brief A simple visual SLAM example for structure from motion +% @author Duy-Nguyen Ta +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Assumptions +% - Landmarks as 8 vertices of a cube: (10,10,10) (-10,10,10) etc... +% - Cameras are on a circle around the cube, pointing at the world origin +% - Each camera sees all landmarks. +% - Visual measurements as 2D points are given, corrupted by Gaussian noise. + +%% Generate simulated data +% 3D landmarks as vertices of a cube +points = {gtsamPoint3([10 10 10]'),... + gtsamPoint3([-10 10 10]'),... + gtsamPoint3([-10 -10 10]'),... + gtsamPoint3([10 -10 10]'),... + gtsamPoint3([10 10 -10]'),... + gtsamPoint3([-10 10 -10]'),... + gtsamPoint3([-10 -10 -10]'),... + gtsamPoint3([10 -10 -10]')}; + +% Camera poses on a circle around the cube, pointing at the world origin +nCameras = 8; +r = 30; +poses = {}; +for i=1:nCameras + theta = i*2*pi/nCameras; + posei = gtsamPose3(... + gtsamRot3([-sin(theta) 0 -cos(theta); + cos(theta) 0 -sin(theta); + 0 -1 0]),... + gtsamPoint3([r*cos(theta), r*sin(theta), 0]')); + poses = [poses {posei}]; +end + +% 2D visual measurements, simulated with Gaussian noise +z = {}; +measurementNoiseSigmas = [0.5,0.5]'; +measurementNoiseSampler = gtsamSharedDiagonal(measurementNoiseSigmas); +K = gtsamCal3_S2(50,50,0,50,50); +for i=1:size(poses,2) + zi = {}; + camera = gtsamSimpleCamera(K,poses{i}); + for j=1:size(points,2) + zi = [zi {camera.project(points{j}).compose(gtsamPoint2(measurementNoiseSampler.sample()))}]; + end + z = [z; zi]; +end + +pointNoiseSigmas = [0.1,0.1,0.1]'; +pointNoiseSampler = gtsamSharedDiagonal(pointNoiseSigmas); + +poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]'; +poseNoiseSampler = gtsamSharedDiagonal(poseNoiseSigmas); + +hold off; + +%% Create the graph (defined in visualSLAM.h, derived from NonlinearFactorGraph) +graph = visualSLAMGraph; + +%% Add factors for all measurements +measurementNoise = gtsamSharedNoiseModel_Sigmas(measurementNoiseSigmas); +for i=1:size(z,1) + for j=1:size(z,2) + graph.addMeasurement(z{i,j}, measurementNoise, symbol('x',i), symbol('l',j), K); + end +end + +%% Add Gaussian priors for a pose and a landmark to constraint the system +posePriorNoise = gtsamSharedNoiseModel_Sigmas(poseNoiseSigmas); +graph.addPosePrior(symbol('x',1), poses{1}, posePriorNoise); +pointPriorNoise = gtsamSharedNoiseModel_Sigmas(pointNoiseSigmas); +graph.addPointPrior(symbol('l',1), points{1}, pointPriorNoise); + +%% Print the graph +graph.print(sprintf('\nFactor graph:\n')); + +%% Initialize to noisy poses and points +initialEstimate = visualSLAMValues; +for i=1:size(poses,2) + initialEstimate.insertPose(symbol('x',i), poses{i}.compose(gtsamPose3_Expmap(poseNoiseSampler.sample()))); +end +for j=1:size(points,2) + initialEstimate.insertPoint(symbol('l',j), points{j}.compose(gtsamPoint3(pointNoiseSampler.sample()))); +end +initialEstimate.print(sprintf('\nInitial estimate:\n ')); + +%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +result = graph.optimize(initialEstimate); +result.print(sprintf('\nFinal result:\n ')); + +%% Query the marginals +marginals = graph.marginals(result); + +%% Plot results with covariance ellipses +hold on; +for j=1:size(points,2) + P = marginals.marginalCovariance(symbol('l',j)); + point_j = result.point(symbol('l',j)); + plot3(point_j.x, point_j.y, point_j.z,'marker','o'); + covarianceEllipse3D([point_j.x;point_j.y;point_j.z],P); +end + +for i=1:size(poses,2) + P = marginals.marginalCovariance(symbol('x',i)); + posei = result.pose(symbol('x',i)) + plotCamera(posei,10); + posei_t = posei.translation() + covarianceEllipse3D([posei_t.x;posei_t.y;posei_t.z],P(4:6,4:6)); +end + diff --git a/examples/matlab/covarianceEllipse.m b/examples/matlab/covarianceEllipse.m index 106a10d6e..491e099c3 100644 --- a/examples/matlab/covarianceEllipse.m +++ b/examples/matlab/covarianceEllipse.m @@ -2,7 +2,7 @@ function covarianceEllipse(x,P,color) % covarianceEllipse: plot a Gaussian as an uncertainty ellipse % Based on Maybeck Vol 1, page 366 % k=2.296 corresponds to 1 std, 68.26% of all probability -% k=11.82 corresponds to 3 std, 99.74% of all probability +% k=11.82 corresponds to 3 std, 99.74% of all probability % % covarianceEllipse(x,P,color) % it is assumed x and y are the first two components of state x @@ -14,21 +14,21 @@ k = 2.296; [ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) ); line(ex,ey,'color',color); -function [x,y] = ellipse(a,b,c); -% ellipse: return the x and y coordinates for an ellipse -% [x,y] = ellipse(a,b,c); -% a, and b are the axes. c is the center - -global ellipse_x ellipse_y -if ~exist('elipse_x') - q =0:2*pi/25:2*pi; - ellipse_x = cos(q); - ellipse_y = sin(q); -end - -points = a*ellipse_x + b*ellipse_y; -x = c(1) + points(1,:); -y = c(2) + points(2,:); -end + function [x,y] = ellipse(a,b,c); + % ellipse: return the x and y coordinates for an ellipse + % [x,y] = ellipse(a,b,c); + % a, and b are the axes. c is the center + + global ellipse_x ellipse_y + if ~exist('elipse_x') + q =0:2*pi/25:2*pi; + ellipse_x = cos(q); + ellipse_y = sin(q); + end + + points = a*ellipse_x + b*ellipse_y; + x = c(1) + points(1,:); + y = c(2) + points(2,:); + end end \ No newline at end of file diff --git a/examples/matlab/covarianceEllipse3D.m b/examples/matlab/covarianceEllipse3D.m new file mode 100644 index 000000000..0ff34c8c8 --- /dev/null +++ b/examples/matlab/covarianceEllipse3D.m @@ -0,0 +1,25 @@ +function covarianceEllipse3D(c,P) +% covarianceEllipse3D: plot a Gaussian as an uncertainty ellipse +% Based on Maybeck Vol 1, page 366 +% k=2.296 corresponds to 1 std, 68.26% of all probability +% k=11.82 corresponds to 3 std, 99.74% of all probability +% +% Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966 + +[e,s] = eig(P); +k = 11.82; +radii = k*sqrt(diag(s)); + +% generate data for "unrotated" ellipsoid +[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3)); + +% rotate data with orientation matrix U and center M +data = kron(e(:,1),xc) + kron(e(:,2),yc) + kron(e(:,3),zc); +n = size(data,2); +x = data(1:n,:)+c(1); y = data(n+1:2*n,:)+c(2); z = data(2*n+1:end,:)+c(3); + +% now plot the rotated ellipse +sc = mesh(x,y,z); +shading interp +alpha(0.5) +axis equal \ No newline at end of file diff --git a/examples/matlab/plot3DTrajectory.m b/examples/matlab/plot3DTrajectory.m index f2c1d8f2f..524b41d33 100644 --- a/examples/matlab/plot3DTrajectory.m +++ b/examples/matlab/plot3DTrajectory.m @@ -1,15 +1,17 @@ -function plot3DTrajectory(values,style,frames) +function plot3DTrajectory(values,style,frames,scale) % plot3DTrajectory if nargin<3,frames=false;end +if nargin<4,scale=0;end plot3(values.xs(),values.ys(),values.zs(),style); hold on if frames + N=values.size; for i=0:N-1 pose = values.pose(i); t = pose.translation; R = pose.rotation.matrix; - quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),'r'); - quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),'g'); - quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),'b'); + quiver3(t.x,t.y,t.z,R(1,1),R(2,1),R(3,1),scale,'r'); + quiver3(t.x,t.y,t.z,R(1,2),R(2,2),R(3,2),scale,'g'); + quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),scale,'b'); end end \ No newline at end of file diff --git a/examples/matlab/plotCamera.m b/examples/matlab/plotCamera.m new file mode 100644 index 000000000..ba352b757 --- /dev/null +++ b/examples/matlab/plotCamera.m @@ -0,0 +1,18 @@ +function plotCamera(pose, axisLength) + C = pose.translation().vector(); + R = pose.rotation().matrix(); + + xAxis = C+R(:,1)*axisLength; + L = [C xAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','r'); + + yAxis = C+R(:,2)*axisLength; + L = [C yAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','g'); + + zAxis = C+R(:,3)*axisLength; + L = [C zAxis]'; + line(L(:,1),L(:,2),L(:,3),'Color','b'); + + axis equal +end \ No newline at end of file diff --git a/examples/matlab/plotPose2.m b/examples/matlab/plotPose2.m new file mode 100644 index 000000000..7dbdebf5a --- /dev/null +++ b/examples/matlab/plotPose2.m @@ -0,0 +1,11 @@ +function plotPose2(p,color,P) +% plotPose2: show a Pose2, possibly with covariance matrix +plot(p.x,p.y,[color '.']); +c = cos(p.theta); +s = sin(p.theta); +quiver(p.x,p.y,c,s,0.1,color); +if nargin>2 + pPp = P(1:2,1:2); % covariance matrix in pose coordinate frame + gRp = [c -s;s c]; % rotation from pose to global + covarianceEllipse([p.x;p.y],gRp*pPp*gRp',color); +end \ No newline at end of file diff --git a/gtsam.h b/gtsam.h index a08c753f1..31bbd4af7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -9,17 +9,17 @@ * Only one Method/Constructor per line * Methods can return * - Eigen types: Matrix, Vector - * - C/C++ basic types: string, bool, size_t, size_t, double, char + * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char * - void * - Any class with which be copied with boost::make_shared() * - boost::shared_ptr of any object type * Limitations on methods * - Parsing does not support overloading - * - There can only be one method with a given name + * - There can only be one method (static or otherwise) with a given name * Arguments to functions any of * - Eigen types: Matrix, Vector * - Eigen types and classes as an optionally const reference - * - C/C++ basic types: string, bool, size_t, size_t, double, char + * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char * - Any class with which be copied with boost::make_shared() (except Eigen) * - boost::shared_ptr of any object type (except Eigen) * Comments can use either C++ or C style, with multiple lines @@ -71,6 +71,7 @@ class Point2 { // Standard Constructors Point2(); Point2(double x, double y); + Point2(Vector v); // Testable void print(string s) const; @@ -174,7 +175,7 @@ class Rot2 { // Group static gtsam::Rot2 identity(); - static gtsam::Rot2 inverse(); + gtsam::Rot2 inverse(); gtsam::Rot2 compose(const gtsam::Rot2& p2) const; gtsam::Rot2 between(const gtsam::Rot2& p2) const; @@ -283,8 +284,8 @@ class Pose2 { static gtsam::Pose2 Expmap(Vector v); static Vector Logmap(const gtsam::Pose2& p); Matrix adjointMap() const; - Vector adjoint() const; - static Matrix wedge(); + Vector adjoint(const Vector& xi) const; + static Matrix wedge(double vx, double vy, double w); // Group Actions on Point2 gtsam::Point2 transform_from(const gtsam::Point2& p) const; @@ -306,7 +307,7 @@ class Pose3 { Pose3(); Pose3(const gtsam::Pose3& pose); Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); - Pose3(const gtsam::Pose2& pose2); + // Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) Pose3(Matrix t); // Testable @@ -344,9 +345,9 @@ class Pose3 { double y() const; double z() const; Matrix matrix() const; - gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; + // gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() double range(const gtsam::Point3& point); - // double range(const gtsam::Pose3& pose); + // double range(const gtsam::Pose3& pose); // FIXME: shadows other range }; class Cal3_S2 { @@ -389,16 +390,6 @@ class Cal3_S2Stereo { void print(string s) const; bool equals(const gtsam::Cal3_S2Stereo& pose, double tol) const; - // Manifold - static size_t Dim(); - size_t dim() const; - gtsam::Cal3_S2Stereo retract(Vector v) const; - Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const; - - // Action on Point2 - gtsam::Point2 calibrate(const gtsam::Point2& p) const; - gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; - // Standard Interface double fx() const; double fy() const; @@ -406,9 +397,6 @@ class Cal3_S2Stereo { double px() const; double py() const; gtsam::Point2 principalPoint() const; - Vector vector() const; - Matrix matrix() const; - Matrix matrix_inverse() const; double baseline() const; }; @@ -442,6 +430,29 @@ class CalibratedCamera { double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods }; + +class SimpleCamera { + // Standard Constructors and Named Constructors + SimpleCamera(); + SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& k); + SimpleCamera(const gtsam::Cal3_S2& k, const gtsam::Pose3& pose); + + // Testable + void print(string s) const; + bool equals(const gtsam::SimpleCamera& camera, double tol) const; + + // Action on Point3 + gtsam::Point2 project(const gtsam::Point3& point); + static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); + + // Backprojection + gtsam::Point3 backproject(const gtsam::Point2& pi, double scale) const; + gtsam::Point3 backproject_from_camera(const gtsam::Point2& pi, double scale) const; + + // Standard Interface + gtsam::Pose3 pose() const; +}; + //************************************************************************* // inference //************************************************************************* @@ -694,6 +705,7 @@ class Graph { void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel); void addConstraint(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel); pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const; + pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate) const; gtsam::Marginals marginals(const pose2SLAM::Values& solution) const; }; @@ -864,6 +876,7 @@ class Values { void print(string s) const; gtsam::Pose3 pose(size_t i); gtsam::Point3 point(size_t j); + bool exists(size_t key); }; class Graph { diff --git a/gtsam/discrete/DiscreteMarginals.h b/gtsam/discrete/DiscreteMarginals.h new file mode 100644 index 000000000..f877a0128 --- /dev/null +++ b/gtsam/discrete/DiscreteMarginals.h @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file DiscreteMarginals.h + * @brief A class for computing marginals in a DiscreteFactorGraph + * @author Abhijit Kundu + * @author Richard Roberts + * @author Frank Dellaert + * @date June 4, 2012 + */ + +#pragma once + +#include + +namespace gtsam { + + /** + * A class for computing marginals of variables in a DiscreteFactorGraph + */ + class DiscreteMarginals { + + protected: + + BayesTree bayesTree_; + + public: + + /** Construct a marginals class. + * @param graph The factor graph defining the full joint density on all variables. + */ + DiscreteMarginals(const DiscreteFactorGraph& graph) { + } + + /** print */ + void print(const std::string& str = "DiscreteMarginals: ") const { + } + + /** Compute the marginal of a single variable */ + DiscreteFactor::shared_ptr operator()(Index variable) const { + DiscreteFactor::shared_ptr p; + return p; + } + + /** Compute the marginal of a single variable */ + Vector marginalProbabilities(Index variable) const { + Vector v; + return v; + } + + }; + +} /* namespace gtsam */ diff --git a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp index f3ca4b59e..667ce9331 100644 --- a/gtsam/discrete/tests/testDiscreteFactorGraph.cpp +++ b/gtsam/discrete/tests/testDiscreteFactorGraph.cpp @@ -10,8 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * testDiscreteFactorGraph.cpp - * + * @file testDiscreteFactorGraph.cpp * @date Feb 14, 2011 * @author Duy-Nguyen Ta */ diff --git a/gtsam/discrete/tests/testDiscreteMarginals.cpp b/gtsam/discrete/tests/testDiscreteMarginals.cpp new file mode 100644 index 000000000..391a5f692 --- /dev/null +++ b/gtsam/discrete/tests/testDiscreteMarginals.cpp @@ -0,0 +1,62 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @ file testDiscreteMarginals.cpp + * @date Feb 14, 2011 + * @author Abhijit Kundu + * @author Richard Roberts + * @author Frank Dellaert + */ + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ + +TEST( DiscreteMarginals, UGM_small ) { + size_t nrStates = 2; + DiscreteKey Cathy(1, nrStates), Heather(2, nrStates), Mark(3, nrStates), + Allison(4, nrStates); + DiscreteFactorGraph graph; + + // add node potentials + graph.add(Cathy, "1 3"); + graph.add(Heather, "9 1"); + graph.add(Mark, "1 3"); + graph.add(Allison, "9 1"); + + // add edge potentials + graph.add(Cathy & Heather, "2 1 1 2"); + graph.add(Heather & Mark, "2 1 1 2"); + graph.add(Mark & Allison, "2 1 1 2"); + + DiscreteMarginals marginals(graph); + + DiscreteFactor::shared_ptr actualC = marginals(Cathy.first); + DiscreteFactor::Values values; + values[Cathy.first] = 0; + EXPECT_DOUBLES_EQUAL( 1.944, (*actualC)(values), 1e-9); + + Vector actualCvector = marginals.marginalProbabilities(Cathy.first); + EXPECT(assert_equal(Vector_(2,0.7,0.3), actualCvector, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 7bbafd7d5..8282800c1 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -26,8 +26,10 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Cal3_S2Stereo: public Cal3_S2 { + class Cal3_S2Stereo { private: + + Cal3_S2 K_; double b_; public: @@ -39,12 +41,12 @@ namespace gtsam { /// default calibration leaves coordinates unchanged Cal3_S2Stereo() : - Cal3_S2(1, 1, 0, 0, 0), b_(1.0) { + K_(1, 1, 0, 0, 0), b_(1.0) { } /// constructor from doubles Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) : - Cal3_S2(fx, fy, s, u0, v0), b_(b) { + K_(fx, fy, s, u0, v0), b_(b) { } /// @} @@ -52,23 +54,41 @@ namespace gtsam { /// @{ void print(const std::string& s = "") const { - gtsam::print(matrix(), s); - std::cout << "Baseline: " << b_ << std::endl; + K_.print(s+"K: "); + std::cout << s << "Baseline: " << b_ << std::endl; } - /// @} + /// Check if equal up to specified tolerance + bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const { + if (fabs(b_ - other.b_) > tol) return false; + return K_.equals(other.K_,tol); + } + + /// @} /// @name Standard Interface /// @{ - //TODO: remove? -// /** -// * Check if equal up to specified tolerance -// */ -// bool equals(const Cal3_S2& K, double tol = 10e-9) const; + /// focal length x + inline double fx() const { return K_.fx();} + /// focal length x + inline double fy() const { return K_.fy();} + /// skew + inline double skew() const { return K_.skew();} - ///TODO: comment + /// image center in x + inline double px() const { return K_.px();} + + /// image center in y + inline double py() const { return K_.py();} + + /// return the principal point + Point2 principalPoint() const { + return K_.principalPoint(); + } + + /// return baseline inline double baseline() const { return b_; } /// @} @@ -81,10 +101,8 @@ namespace gtsam { template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3_S2Stereo", - boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(b_); - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Cal3_S2); } /// @} diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 943409b4b..587312062 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -256,13 +256,13 @@ namespace gtsam { * backproject a 2-dimensional point to a 3-dimension point */ - inline Point3 backproject(const Point2& pi, const double scale) const { + inline Point3 backproject(const Point2& pi, double scale) const { const Point2 pn = k_.calibrate(pi); const Point3 pc(pn.x()*scale, pn.y()*scale, scale); return pose_.transform_from(pc); } - inline Point3 backproject_from_camera(const Point2& pi, const double scale) const { + inline Point3 backproject_from_camera(const Point2& pi, double scale) const { return backproject(pi, scale); } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index f5e07e66c..4c278fab9 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -71,16 +71,6 @@ public: return project(point, H1, H2); } - /* - * backproject using left camera calibration, up to scale only - * i.e. does not rely on baseline - */ - Point3 backproject(const Point2& projection, const double scale) const { - Point2 intrinsic = K_->calibrate(projection); - Point3 cameraPoint = Point3(intrinsic.x() * scale, intrinsic.y() * scale, scale);; - return pose().transform_from(cameraPoint); - } - Point3 backproject(const StereoPoint2& z) const { Vector measured = z.vector(); double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]); diff --git a/gtsam/linear/IterativeOptimizationParameters.h b/gtsam/linear/IterativeOptimizationParameters.h index f6b1877ce..88303fa40 100644 --- a/gtsam/linear/IterativeOptimizationParameters.h +++ b/gtsam/linear/IterativeOptimizationParameters.h @@ -113,7 +113,9 @@ struct PreconditionerParameters { std::cout << "PreconditionerParameters: " << "kernel = " << kernelStr[kernel_] - << ", type = " << typeStr[type_] << std::endl; + << ", type = " << typeStr[type_] + << ", verbosity = " << verbosity_ + << std::endl; combinatorial_.print(); } }; @@ -167,6 +169,7 @@ struct ConjugateGradientParameters { << ", eps_rel = " << epsilon_rel_ << ", eps_abs = " << epsilon_abs_ << ", degree = " << degree_ + << ", verbosity = " << verbosity_ << std::endl; } }; @@ -180,8 +183,8 @@ public: PreconditionerParameters preconditioner_; ConjugateGradientParameters cg_; - enum Kernel { PCG = 0, LSPCG } kernel_ ; /* Iterative Method Kernel */ - enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */ + enum Kernel { PCG = 0, LSPCG = 1 } kernel_ ; ///< Iterative Method Kernel + enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; ///< Verbosity public: @@ -221,7 +224,8 @@ public: const std::string kernelStr[2] = {"pcg", "lspcg"}; std::cout << s << std::endl << "IterativeOptimizationParameters: " - << "kernel = " << kernelStr[kernel_] << std::endl; + << "kernel = " << kernelStr[kernel_] + << ", verbosity = " << verbosity_ << std::endl; cg_.print(); preconditioner_.print(); diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 9b07d8e07..58e83d595 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -11,9 +11,8 @@ #pragma once -#include #include -#include +#include namespace gtsam { @@ -21,25 +20,23 @@ class IterativeSolver { public: - typedef boost::shared_ptr shared_ptr; typedef IterativeOptimizationParameters Parameters; protected: - Parameters::shared_ptr parameters_ ; + Parameters parameters_ ; public: - IterativeSolver(): parameters_(new Parameters()) {} + IterativeSolver(): parameters_() {} IterativeSolver(const IterativeSolver &solver) : parameters_(solver.parameters_) {} - IterativeSolver(const Parameters::shared_ptr& parameters) : parameters_(parameters) {} - IterativeSolver(const Parameters ¶meters) : parameters_(new Parameters(parameters)) {} + IterativeSolver(const Parameters ¶meters) : parameters_(parameters) {} virtual ~IterativeSolver() {} virtual VectorValues::shared_ptr optimize () = 0; - inline Parameters::shared_ptr parameters() { return parameters_ ; } + inline const Parameters& parameters() const { return parameters_ ; } }; } diff --git a/gtsam/linear/SimpleSPCGSolver.cpp b/gtsam/linear/SimpleSPCGSolver.cpp index e7cef9e64..ae7c9581c 100644 --- a/gtsam/linear/SimpleSPCGSolver.cpp +++ b/gtsam/linear/SimpleSPCGSolver.cpp @@ -42,7 +42,7 @@ std::vector extractColSpec_(const FactorGraph& gfg, cons return spec; } -SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr ¶meters) +SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters) : Base(parameters) { std::vector colSpec = extractColSpec_(gfg, VariableIndex(gfg)); @@ -97,13 +97,14 @@ VectorValues::shared_ptr SimpleSPCGSolver::optimize (const VectorValues &initial double gamma = s.vector().squaredNorm(), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ; const double threshold = - ::max(parameters_->epsilon_abs(), - parameters_->epsilon() * parameters_->epsilon() * gamma); - const size_t iMaxIterations = parameters_->maxIterations(); + ::max(parameters_.epsilon_abs(), + parameters_.epsilon() * parameters_.epsilon() * gamma); + const size_t iMaxIterations = parameters_.maxIterations(); + const ConjugateGradientParameters::Verbosity verbosity = parameters_.cg_.verbosity(); - if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR ) - cout << "[SimpleSPCGSolver] epsilon = " << parameters_->epsilon() - << ", max = " << parameters_->maxIterations() + if ( verbosity >= ConjugateGradientParameters::ERROR ) + cout << "[SimpleSPCGSolver] epsilon = " << parameters_.epsilon() + << ", max = " << parameters_.maxIterations() << ", ||r0|| = " << std::sqrt(gamma) << ", threshold = " << threshold << std::endl; @@ -120,14 +121,14 @@ VectorValues::shared_ptr SimpleSPCGSolver::optimize (const VectorValues &initial p.vector() = s.vector() + beta * p.vector(); gamma = new_gamma ; - if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR) { + if ( verbosity >= ConjugateGradientParameters::ERROR) { cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl; } if ( gamma < threshold ) break ; } // k - if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR ) + if ( verbosity >= ConjugateGradientParameters::ERROR ) cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl; /* transform y back to x */ diff --git a/gtsam/linear/SimpleSPCGSolver.h b/gtsam/linear/SimpleSPCGSolver.h index 04729fc1a..570169e7b 100644 --- a/gtsam/linear/SimpleSPCGSolver.h +++ b/gtsam/linear/SimpleSPCGSolver.h @@ -55,7 +55,7 @@ protected: public: - SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr ¶meters); + SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters); virtual ~SimpleSPCGSolver() {} virtual VectorValues::shared_ptr optimize () {return optimize(*y0_);} diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 042d4333d..af44eee93 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -74,7 +74,8 @@ void LevenbergMarquardtOptimizer::iterate() { delta = gtsam::optimize(*EliminationTree::Create(dampedSystem)->eliminate(params_.getEliminationFunction())); } else if ( params_.isCG() ) { - SimpleSPCGSolver solver(dampedSystem, *params_.iterativeParams); + IterativeOptimizationParameters::shared_ptr params(!params_.iterativeParams ? boost::make_shared() : params_.iterativeParams); + SimpleSPCGSolver solver(dampedSystem, *params); delta = *solver.optimize(); } else { diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 73c150695..a5a838881 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -118,7 +118,7 @@ public: /** * Calculate the error of the factor - * This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian. + * This is typically equal to log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/sigma^2 \f$ in case of Gaussian. * You can override this for systems with unusual noise models. */ virtual double error(const Values& c) const = 0; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 82b364ddf..429bf0102 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -49,7 +49,7 @@ namespace gtsam { /** return keys in some random order */ std::set keys() const; - /** unnormalized error */ + /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */ double error(const Values& c) const; /** Unnormalized probability. O(n) */ diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index ea54dd344..156440f06 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -31,13 +31,13 @@ public: MULTIFRONTAL_QR, SEQUENTIAL_CHOLESKY, SEQUENTIAL_QR, - CHOLMOD, /* Experimental Flag */ CG, /* Experimental Flag */ + CHOLMOD, /* Experimental Flag */ }; LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) - boost::optional iterativeParams; ///< The container for iterativeOptimization parameters. + IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {} diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index 49ab37111..110a50551 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -85,6 +85,13 @@ namespace pose2SLAM { return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize(); } + Values Graph::optimizeSPCG(const Values& initialEstimate) const { + LevenbergMarquardtParams params; + params.linearSolverType = SuccessiveLinearizationParams::CG; + return LevenbergMarquardtOptimizer(*this, initialEstimate, params).optimize(); + } + + /* ************************************************************************* */ } // pose2SLAM diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index d7ebd3323..71710f823 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -107,6 +107,7 @@ namespace pose2SLAM { /// Optimize Values optimize(const Values& initialEstimate) const; + Values optimizeSPCG(const Values& initialEstimate) const; /// Return a Marginals object Marginals marginals(const Values& solution) const { diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index f71d0c4ca..7674ed1e3 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -72,6 +72,9 @@ namespace visualSLAM { /// get a point Point3 point(Key j) const { return at(j); } + /// check if value with specified key exists + bool exists(Key i) const { return gtsam::Values::exists(i); } + }; /** diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index b50667f48..54e95a599 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -71,7 +71,8 @@ if (GTSAM_BUILD_WRAP) include(GtsamMatlabWrap) # Wrap codegen - #usage: wrap mexExtension interfacePath moduleName toolboxPath + #usage: wrap mexExecutable mexExtension interfacePath moduleName toolboxPath [mexFlags] + # mexExecutable : command to execute mex if on path, use 'mex' # mexExtension : OS/CPU-dependent extension for MEX binaries # interfacePath : *absolute* path to directory of module interface file # moduleName : the name of the module, interface file must be called moduleName.h @@ -87,18 +88,29 @@ if (GTSAM_BUILD_WRAP) # Code generation command add_custom_target(wrap_gtsam_unstable ALL COMMAND - ${CMAKE_BINARY_DIR}/wrap/wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR} ${moduleName} ${toolbox_path} "${mexFlags}" + ${CMAKE_BINARY_DIR}/wrap/wrap + ${MEX_COMMAND} + ${GTSAM_MEX_BIN_EXTENSION} + ${CMAKE_CURRENT_SOURCE_DIR} + ${moduleName} + ${toolbox_path} + "${mexFlags}" DEPENDS wrap) # Build command - # Experimental: requires matlab to be on your path if (GTSAM_ENABLE_BUILD_MEX_BINARIES) # Actually compile the mex files when building the library - set(TOOLBOX_MAKE_FLAGS "-j2") - add_custom_target(wrap_gtsam_unstable_build - COMMAND make ${TOOLBOX_MAKE_FLAGS} - WORKING_DIRECTORY ${toolbox_path} - DEPENDS wrap_gtsam_unstable) + if (GTSAM_ENABLE_BUILD_MEX_BINARIES_ALL) + add_custom_target(wrap_gtsam_unstable_build ALL + COMMAND make ${GTSAM_BUILD_MEX_BINARY_FLAGS} + WORKING_DIRECTORY ${toolbox_path} + DEPENDS wrap_gtsam_unstable) + else() + add_custom_target(wrap_gtsam_unstable_build + COMMAND make ${GTSAM_BUILD_MEX_BINARY_FLAGS} + WORKING_DIRECTORY ${toolbox_path} + DEPENDS wrap_gtsam_unstable) + endif() endif (GTSAM_ENABLE_BUILD_MEX_BINARIES) if (GTSAM_INSTALL_MATLAB_TOOLBOX) diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 376706e7f..91377da8c 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -2,15 +2,14 @@ * Matlab toolbox interface definition for gtsam_unstable */ -// Most things are in the gtsam namespace -using namespace gtsam; - // specify the classes from gtsam we are using class gtsam::Point3; class gtsam::Rot3; class gtsam::Pose3; class gtsam::SharedNoiseModel; +namespace gtsam { + #include class PoseRTV { PoseRTV(); @@ -22,7 +21,7 @@ class PoseRTV { PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz); // testable - bool equals(const PoseRTV& other, double tol) const; + bool equals(const gtsam::PoseRTV& other, double tol) const; void print(string s) const; // access @@ -39,27 +38,29 @@ class PoseRTV { // manifold/Lie static size_t Dim(); size_t dim() const; - PoseRTV retract(Vector v) const; - Vector localCoordinates(const PoseRTV& p) const; - static PoseRTV Expmap(Vector v); - static Vector Logmap(const PoseRTV& p); - PoseRTV inverse() const; - PoseRTV compose(const PoseRTV& p) const; - PoseRTV between(const PoseRTV& p) const; + gtsam::PoseRTV retract(Vector v) const; + Vector localCoordinates(const gtsam::PoseRTV& p) const; + static gtsam::PoseRTV Expmap(Vector v); + static Vector Logmap(const gtsam::PoseRTV& p); + gtsam::PoseRTV inverse() const; + gtsam::PoseRTV compose(const gtsam::PoseRTV& p) const; + gtsam::PoseRTV between(const gtsam::PoseRTV& p) const; // measurement - double range(const PoseRTV& other) const; - PoseRTV transformed_from(const gtsam::Pose3& trans) const; + double range(const gtsam::PoseRTV& other) const; + gtsam::PoseRTV transformed_from(const gtsam::Pose3& trans) const; // IMU/dynamics - PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const; - PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const; - PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const; - Vector imuPrediction(const PoseRTV& x2, double dt) const; - gtsam::Point3 translationIntegration(const PoseRTV& x2, double dt) const; - Vector translationIntegrationVec(const PoseRTV& x2, double dt) const; + gtsam::PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const; + gtsam::PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const; + gtsam::PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const; + Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const; + gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const; + Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const; }; +}///\namespace gtsam + #include namespace imu { @@ -67,8 +68,8 @@ class Values { Values(); void print(string s) const; - void insertPose(size_t key, const PoseRTV& pose); - PoseRTV pose(size_t key) const; + void insertPose(size_t key, const gtsam::PoseRTV& pose); + gtsam::PoseRTV pose(size_t key) const; }; class Graph { @@ -76,8 +77,8 @@ class Graph { void print(string s) const; // prior factors - void addPrior(size_t key, const PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel); - void addConstraint(size_t key, const PoseRTV& pose); + void addPrior(size_t key, const gtsam::PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel); + void addConstraint(size_t key, const gtsam::PoseRTV& pose); void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel); // inertial factors @@ -86,7 +87,7 @@ class Graph { void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel); // other measurements - void addBetween(size_t key1, size_t key2, const PoseRTV& z, const gtsam::SharedNoiseModel& noiseModel); + void addBetween(size_t key1, size_t key2, const gtsam::PoseRTV& z, const gtsam::SharedNoiseModel& noiseModel); void addRange(size_t key1, size_t key2, double z, const gtsam::SharedNoiseModel& noiseModel); // optimization diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index b03431d72..af9097ea4 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -30,7 +30,7 @@ string Argument::matlabClass(const string& delim) const { string result; BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; - if (type=="string") + if (type=="string" || type=="unsigned char" || type=="char") return result + "char"; if (type=="bool" || type=="int" || type=="size_t" || type=="Vector" || type=="Matrix") return result + "double"; diff --git a/wrap/CMakeLists.txt b/wrap/CMakeLists.txt index 57a82ba8a..8f44e24cf 100644 --- a/wrap/CMakeLists.txt +++ b/wrap/CMakeLists.txt @@ -23,14 +23,15 @@ if (GTSAM_BUILD_TESTS) endif(GTSAM_BUILD_TESTS) # Wrap codegen -#usage: wrap mexExtension interfacePath moduleName toolboxPath +#usage: wrap mexExecutable mexExtension interfacePath moduleName toolboxPath [mexFlags] +# mexExecutable : command to execute mex if on path, use 'mex' # mexExtension : OS/CPU-dependent extension for MEX binaries # interfacePath : *absolute* path to directory of module interface file # moduleName : the name of the module, interface file must be called moduleName.h # toolboxPath : the directory in which to generate the wrappers # [mexFlags] : extra flags for the mex command -set(mexFlags "-I${Boost_INCLUDE_DIR} -I${CMAKE_INSTALL_PREFIX}/include -I${CMAKE_INSTALL_PREFIX}/include/gtsam -I${CMAKE_INSTALL_PREFIX}/include/gtsam/base -I${CMAKE_INSTALL_PREFIX}/include/gtsam/geometry -I${CMAKE_INSTALL_PREFIX}/include/gtsam/linear -I${CMAKE_INSTALL_PREFIX}/include/gtsam/nonlinear -I${CMAKE_INSTALL_PREFIX}/include/gtsam/slam -L${CMAKE_INSTALL_PREFIX}/lib -lgtsam") +set(mexFlags "-I${Boost_INCLUDE_DIR} -I${CMAKE_INSTALL_PREFIX}/include -I${CMAKE_INSTALL_PREFIX}/include/gtsam -I${CMAKE_INSTALL_PREFIX}/include/gtsam/base -I${CMAKE_INSTALL_PREFIX}/include/gtsam/geometry -I${CMAKE_INSTALL_PREFIX}/include/gtsam/linear -I${CMAKE_INSTALL_PREFIX}/include/gtsam/discrete -I${CMAKE_INSTALL_PREFIX}/include/gtsam/inference -I${CMAKE_INSTALL_PREFIX}/include/gtsam/nonlinear -I${CMAKE_INSTALL_PREFIX}/include/gtsam/slam -L${CMAKE_INSTALL_PREFIX}/lib -lgtsam") if(MSVC OR CYGWIN OR WINGW) set(mexFlags "${mexFlags} LINKFLAGS='$LINKFLAGS /LIBPATH:${Boost_LIBRARY_DIRS}'") endif() @@ -48,24 +49,39 @@ endif() # Code generation command get_property(WRAP_EXE TARGET wrap PROPERTY LOCATION) add_custom_target(wrap_gtsam ALL COMMAND - ${WRAP_EXE} ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR}/../ ${moduleName} ${toolbox_path} "${mexFlags}" + ${WRAP_EXE} + ${MEX_COMMAND} + ${GTSAM_MEX_BIN_EXTENSION} + ${CMAKE_CURRENT_SOURCE_DIR}/../ + ${moduleName} + ${toolbox_path} + "${mexFlags}" DEPENDS wrap) # Build command -# Experimental: requires matlab to be on your path if (GTSAM_ENABLE_BUILD_MEX_BINARIES) # Actually compile the mex files when building the library - set(TOOLBOX_MAKE_FLAGS "-j2") - add_custom_target(wrap_gtsam_build - COMMAND make ${TOOLBOX_MAKE_FLAGS} - WORKING_DIRECTORY ${toolbox_path} - DEPENDS wrap_gtsam) + # TODO: pass correct make flags from parent process + message(STATUS "Building Matlab MEX binaries for toolbox with flags ${GTSAM_BUILD_MEX_BINARY_FLAGS}") + if (GTSAM_ENABLE_BUILD_MEX_BINARIES_ALL) + add_custom_target(wrap_gtsam_build ALL + COMMAND make ${GTSAM_BUILD_MEX_BINARY_FLAGS} + WORKING_DIRECTORY ${toolbox_path} + DEPENDS wrap_gtsam) + else() + add_custom_target(wrap_gtsam_build + COMMAND make ${GTSAM_BUILD_MEX_BINARY_FLAGS} + WORKING_DIRECTORY ${toolbox_path} + DEPENDS wrap_gtsam) + endif() endif (GTSAM_ENABLE_BUILD_MEX_BINARIES) set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox") if (GTSAM_INSTALL_MATLAB_TOOLBOX) # Primary toolbox files + # Note that we copy the entire contents of the folder blindly - this is because + # the generated files won't exist at configuration time message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}") install(DIRECTORY DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}) # make an empty folder # exploit need for trailing slash to specify a full folder, rather than just its contents to copy @@ -78,10 +94,11 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) install(FILES ${matlab_examples} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/examples) message(STATUS "Installing Matlab Toolbox Examples (Data)") - set(data_excludes "${CMAKE_SOURCE_DIR}/examples/Data/.svn") - file(GLOB matlab_examples_data "${CMAKE_SOURCE_DIR}/examples/Data/*.*") - list(REMOVE_ITEM matlab_examples_data ${data_excludes}) - install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/examples/Data) + # Data files: *.graph and *.txt + file(GLOB matlab_examples_data_graph "${CMAKE_SOURCE_DIR}/examples/Data/*.graph") + file(GLOB matlab_examples_data_txt "${CMAKE_SOURCE_DIR}/examples/Data/*.txt") + set(matlab_examples_data ${matlab_examples_data_graph} ${matlab_examples_data_txt}) + install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/Data) endif (GTSAM_INSTALL_MATLAB_EXAMPLES) # Tests diff --git a/wrap/Module.cpp b/wrap/Module.cpp index af2238038..e0771351a 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -279,7 +279,7 @@ void verifyReturnTypes(const vector& validtypes, const vector& vt) { } /* ************************************************************************* */ -void Module::matlab_code(const string& toolboxPath, +void Module::matlab_code(const string& mexCommand, const string& toolboxPath, const string& mexExt, const string& mexFlags) const { fs::create_directories(toolboxPath); @@ -299,7 +299,7 @@ void Module::matlab_code(const string& toolboxPath, makeModuleMfile.oss << "clear delims" << endl; makeModuleMfile.oss << "addpath(toolboxpath);" << endl << endl; - makeModuleMakefile.oss << "\nMEX = mex\n"; + makeModuleMakefile.oss << "\nMEX = " << mexCommand << "\n"; makeModuleMakefile.oss << "MEXENDING = " << mexExt << "\n"; makeModuleMakefile.oss << "mex_flags = " << mexFlags << "\n\n"; diff --git a/wrap/Module.h b/wrap/Module.h index 3e97d9ba2..4b650728d 100644 --- a/wrap/Module.h +++ b/wrap/Module.h @@ -40,7 +40,9 @@ struct Module { bool enable_verbose=true); /// MATLAB code generation: - void matlab_code(const std::string& path, + void matlab_code( + const std::string& mexCommand, + const std::string& path, const std::string& mexExt, const std::string& mexFlags) const; }; diff --git a/wrap/README b/wrap/README index 6e7acc863..92fd1967c 100644 --- a/wrap/README +++ b/wrap/README @@ -1,14 +1,17 @@ Frank Dellaert October 2011 -The wrap library wraps the GTSAM library into a MATLAB toolbox. +The wrap library wraps the GTSAM library into a MATLAB toolbox. It was designed to be more general than just wrapping GTSAM, but a small amount of GTSAM specific code exists in matlab.h, the include file that is included by the -mex files. In addition, the current makefile (Oct 11) is GTSAM specific. +mex files. The GTSAM-specific functionality consists primarily of handling of +Eigen Matrix and Vector classes. + +For notes on creating a wrap interface, see gtsam.h for what features can be +wrapped into a toolbox, as well as the current state of the toolbox for gtsam. +For more technical details on the interface, please read comments in matlab.h -The classes and methods to be wrapped are specified in gtsam.h -This tool will not wrap arbitrary methods. Please read comments in matlab.h Some good things to know: OBJECT CREATION @@ -24,7 +27,5 @@ METHOD (AND CONSTRUCTOR) ARGUMENTS generation of the correct conversion routines unwrap and unwrap - passing classes as arguments works, provided they are passed by reference. This triggers a call to unwrap_shared_ptr -- GTSAM specific: keys will be cast automatically from strings via GTSAM_magic. This - allows us to not have to declare all key types in MATLAB. Just replace key arguments with - the (non-const, non-reference) string type + \ No newline at end of file diff --git a/wrap/tests/expected_namespaces/@ClassD/ClassD.m b/wrap/tests/expected_namespaces/@ClassD/ClassD.m index 8b78750d9..9e3ddf132 100644 --- a/wrap/tests/expected_namespaces/@ClassD/ClassD.m +++ b/wrap/tests/expected_namespaces/@ClassD/ClassD.m @@ -9,7 +9,7 @@ classdef ClassD < handle if nargin ~= 13 && obj.self == 0, error('ClassD constructor failed'); end end function delete(obj) - delete_ClassD(obj) + delete_ClassD(obj); end function display(obj), obj.print(''); end function disp(obj), obj.display; end diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 91b2161f7..db3ac8982 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -63,7 +63,7 @@ TEST( wrap, check_exception ) { string path = topdir + "/wrap/tests"; Module module(path.c_str(), "testDependencies",enable_verbose); - CHECK_EXCEPTION(module.matlab_code("actual_deps", "mexa64", "-O5"), DependencyMissing); + CHECK_EXCEPTION(module.matlab_code("mex", "actual_deps", "mexa64", "-O5"), DependencyMissing); } /* ************************************************************************* */ @@ -214,7 +214,7 @@ TEST( wrap, matlab_code_namespaces ) { // emit MATLAB code string exp_path = path + "/tests/expected_namespaces/"; string act_path = "actual_namespaces/"; - module.matlab_code("actual_namespaces", "mexa64", "-O5"); + module.matlab_code("mex", "actual_namespaces", "mexa64", "-O5"); EXPECT(files_equal(exp_path + "new_ClassD_.cpp" , act_path + "new_ClassD_.cpp" )); EXPECT(files_equal(exp_path + "new_ClassD_.m" , act_path + "new_ClassD_.m" )); @@ -267,7 +267,7 @@ TEST( wrap, matlab_code ) { // emit MATLAB code // make_geometry will not compile, use make testwrap to generate real make - module.matlab_code("actual", "mexa64", "-O5"); + module.matlab_code("mex", "actual", "mexa64", "-O5"); EXPECT(files_equal(path + "/tests/expected/@Point2/Point2.m" , "actual/@Point2/Point2.m" )); EXPECT(files_equal(path + "/tests/expected/@Point2/x.cpp" , "actual/@Point2/x.cpp" )); diff --git a/wrap/wrap.cpp b/wrap/wrap.cpp index f55927a5e..eabba647e 100644 --- a/wrap/wrap.cpp +++ b/wrap/wrap.cpp @@ -24,6 +24,7 @@ using namespace std; /** * Top-level function to wrap a module + * @param mexCommand is a sufficiently qualified command to execute mex within a makefile * @param mexExt is the extension for mex binaries for this os/cpu * @param interfacePath path to where interface file lives, e.g., borg/gtsam * @param moduleName name of the module to be generated e.g. gtsam @@ -31,7 +32,9 @@ using namespace std; * @param nameSpace e.g. gtsam * @param mexFlags extra arguments for mex script, i.e., include flags etc... */ -void generate_matlab_toolbox(const string& mexExt, +void generate_matlab_toolbox( + const string& mexCommand, + const string& mexExt, const string& interfacePath, const string& moduleName, const string& toolboxPath, @@ -42,13 +45,14 @@ void generate_matlab_toolbox(const string& mexExt, wrap::Module module(interfacePath, moduleName, true); // Then emit MATLAB code - module.matlab_code(toolboxPath,mexExt,mexFlags); + module.matlab_code(mexCommand,toolboxPath,mexExt,mexFlags); } /** Displays usage information */ void usage() { cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl; - cerr << "usage: wrap mexExtension interfacePath moduleName toolboxPath [mexFlags]" << endl; + cerr << "usage: wrap mexExecutable mexExtension interfacePath moduleName toolboxPath [mexFlags]" << endl; + cerr << " mexExecutable : command to execute mex if on path, use 'mex'" << endl; cerr << " mexExtension : OS/CPU-dependent extension for MEX binaries" << endl; cerr << " interfacePath : *absolute* path to directory of module interface file" << endl; cerr << " moduleName : the name of the module, interface file must be called moduleName.h" << endl; @@ -61,7 +65,7 @@ void usage() { * Typically called from "make all" using appropriate arguments */ int main(int argc, const char* argv[]) { - if (argc<6 || argc>7) { + if (argc<7 || argc>8) { cerr << "Invalid arguments:\n"; for (int i=0; i