763 lines
		
	
	
		
			15 KiB
		
	
	
	
		
			Plaintext
		
	
	
			
		
		
	
	
			763 lines
		
	
	
		
			15 KiB
		
	
	
	
		
			Plaintext
		
	
	
#LyX 2.0 created this file. For more info see http://www.lyx.org/
 | 
						|
\lyxformat 413
 | 
						|
\begin_document
 | 
						|
\begin_header
 | 
						|
\textclass article
 | 
						|
\begin_preamble
 | 
						|
\usepackage{amssymb}
 | 
						|
\end_preamble
 | 
						|
\use_default_options true
 | 
						|
\maintain_unincluded_children false
 | 
						|
\language english
 | 
						|
\language_package default
 | 
						|
\inputencoding auto
 | 
						|
\fontencoding global
 | 
						|
\font_roman default
 | 
						|
\font_sans default
 | 
						|
\font_typewriter default
 | 
						|
\font_default_family default
 | 
						|
\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 default
 | 
						|
\spacing single
 | 
						|
\use_hyperref false
 | 
						|
\papersize default
 | 
						|
\use_geometry false
 | 
						|
\use_amsmath 1
 | 
						|
\use_esint 1
 | 
						|
\use_mhchem 1
 | 
						|
\use_mathdots 1
 | 
						|
\cite_engine basic
 | 
						|
\use_bibtopic false
 | 
						|
\use_indices false
 | 
						|
\paperorientation portrait
 | 
						|
\suppress_date false
 | 
						|
\use_refstyle 1
 | 
						|
\index Index
 | 
						|
\shortcut idx
 | 
						|
\color #008000
 | 
						|
\end_index
 | 
						|
\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 Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\SE}[1]{\mathbb{SE}\left(#1\right)}
 | 
						|
{\mathbb{SE}\left(#1\right)}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\se}[1]{\mathfrak{se}\left(#1\right)}
 | 
						|
{\mathfrak{se}\left(#1\right)}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\SO}[1]{\mathbb{SO}\left(#1\right)}
 | 
						|
{\mathbb{SO}\left(#1\right)}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\so}[1]{\mathfrak{so}\left(#1\right)}
 | 
						|
{\mathfrak{so}\left(#1\right)}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\R}[1]{\mathbb{R}^{#1}}
 | 
						|
{\mathbb{R}^{#1}}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\prob}[2]{#1\hspace{0.1em}|\hspace{0.1em}#2}
 | 
						|
{#1\hspace{0.1em}|\hspace{0.1em}#2}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\norm}[1]{\left\Vert #1\right\Vert }
 | 
						|
{\left\Vert #1\right\Vert }
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\t}{\mathsf{T}}
 | 
						|
{\mathsf{T}}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset ERT
 | 
						|
status open
 | 
						|
 | 
						|
\begin_layout Plain Layout
 | 
						|
 | 
						|
 | 
						|
\backslash
 | 
						|
newcommand{
 | 
						|
\backslash
 | 
						|
smallequals}{
 | 
						|
\backslash
 | 
						|
mbox{
 | 
						|
\backslash
 | 
						|
raisebox{0.2ex}{
 | 
						|
\backslash
 | 
						|
fontsize{8}{10}
 | 
						|
\backslash
 | 
						|
selectfont $=$}}}
 | 
						|
\end_layout
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\smeq}{\smallequals}
 | 
						|
{{\scriptstyle =}}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\th}[1]{#1^{\mathrm{th}}}
 | 
						|
{#1^{\mathrm{th}}}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\defeq}{\stackrel{\Delta}{=}}
 | 
						|
{\stackrel{\Delta}{=}}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\im}{\mathcal{I}}
 | 
						|
{\mathcal{I}}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\lin}[1]{\overset{{\scriptscriptstyle \circ}}{#1}}
 | 
						|
{\overset{{\scriptscriptstyle \circ}}{#1}}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset FormulaMacro
 | 
						|
\newcommand{\lins}[3]{\overset{{\scriptscriptstyle \circ}}{#1}\vphantom{#1}_{#3}^{#2}}
 | 
						|
{\overset{{\scriptscriptstyle \circ}}{#1}\vphantom{#1}_{#3}^{#2}}
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Section
 | 
						|
Overview of Trust-region Methods
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
For nice figures, see
 | 
						|
\begin_inset space ~
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\begin_inset CommandInset citation
 | 
						|
LatexCommand cite
 | 
						|
key "Hauser06lecture"
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 (in our /net/hp223/borg/Literature folder).
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
We just deal here with a small subset of trust-region methods, specifically
 | 
						|
 approximating the cost function as quadratic using Newton's method, and
 | 
						|
 using the Dogleg method and later to include Steihaug's method.
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
The overall goal of a nonlinear optimization method is to iteratively find
 | 
						|
 a local minimum of a nonlinear function
 | 
						|
\begin_inset Formula 
 | 
						|
\[
 | 
						|
\hat{x}=\arg\min_{x}f\left(x\right)
 | 
						|
\]
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
where 
 | 
						|
\begin_inset Formula $f\left(x\right)\to\mathbb{R}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is a scalar function.
 | 
						|
 In GTSAM, the variables 
 | 
						|
\begin_inset Formula $x$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 could be manifold or Lie group elements, so in this document we only work
 | 
						|
 with 
 | 
						|
\emph on
 | 
						|
increments
 | 
						|
\emph default
 | 
						|
 
 | 
						|
\begin_inset Formula $\delta x\in\R n$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 in the tangent space.
 | 
						|
 In this document we specifically deal with 
 | 
						|
\emph on
 | 
						|
trust-region
 | 
						|
\emph default
 | 
						|
 methods, which at every iteration attempt to find a good increment 
 | 
						|
\begin_inset Formula $\norm{\delta x}\leq\Delta$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 within the 
 | 
						|
\begin_inset Quotes eld
 | 
						|
\end_inset
 | 
						|
 | 
						|
trust radius
 | 
						|
\begin_inset Quotes erd
 | 
						|
\end_inset
 | 
						|
 | 
						|
 
 | 
						|
\begin_inset Formula $\Delta$
 | 
						|
\end_inset
 | 
						|
 | 
						|
.
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
Further, most nonlinear optimization methods, including trust region methods,
 | 
						|
 deal with an approximate problem at every iteration.
 | 
						|
 Although there are other choices (such as quasi-Newton), the Newton's method
 | 
						|
 approximation is, given an estimate 
 | 
						|
\begin_inset Formula $x^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 of the variables 
 | 
						|
\begin_inset Formula $x$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, 
 | 
						|
\begin_inset Formula 
 | 
						|
\begin{equation}
 | 
						|
f\left(x^{\left(k\right)}\oplus\delta x\right)\approx M^{\left(k\right)}\left(\delta x\right)=f^{\left(k\right)}+g^{\left(k\right)\t}\delta x+\frac{1}{2}\delta x^{\t}G^{\left(k\right)}\delta x\text{,}\label{eq:M-approx}
 | 
						|
\end{equation}
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
where 
 | 
						|
\begin_inset Formula $f^{\left(k\right)}=f\left(x^{\left(k\right)}\right)$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is the function at 
 | 
						|
\begin_inset Formula $x^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, 
 | 
						|
\begin_inset Formula $g^{\left(x\right)}=\left.\frac{\partial f}{\partial x}\right|_{x^{\left(k\right)}}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is its gradient, and 
 | 
						|
\begin_inset Formula $G^{\left(k\right)}=\left.\frac{\partial^{2}f}{\partial x^{2}}\right|_{x^{\left(k\right)}}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is its Hessian (or an approximation of the Hessian).
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
Trust-region methods adaptively adjust the trust radius 
 | 
						|
\begin_inset Formula $\Delta$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 so that within it, 
 | 
						|
\begin_inset Formula $M$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is a good approximation of 
 | 
						|
\begin_inset Formula $f$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, and then never step beyond the trust radius in each iteration.
 | 
						|
 When the true minimum is within the trust region, they converge quadratically
 | 
						|
 like Newton's method.
 | 
						|
 At each iteration 
 | 
						|
\begin_inset Formula $k$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, they solve the 
 | 
						|
\emph on
 | 
						|
trust-region subproblem
 | 
						|
\emph default
 | 
						|
 to find a proposed update 
 | 
						|
\begin_inset Formula $\delta x$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 inside the trust radius 
 | 
						|
\begin_inset Formula $\Delta$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, which decreases the approximate function 
 | 
						|
\begin_inset Formula $M^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 as much as possible.
 | 
						|
 The proposed update is only accepted if the true function 
 | 
						|
\begin_inset Formula $f$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 decreases as well.
 | 
						|
 If the decrease of 
 | 
						|
\begin_inset Formula $M$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 matches the decrease of 
 | 
						|
\begin_inset Formula $f$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 well, the size of the trust region is increased, while if the match is
 | 
						|
 not close the trust region size is decreased.
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
Minimizing Eq.
 | 
						|
\begin_inset space ~
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\begin_inset CommandInset ref
 | 
						|
LatexCommand ref
 | 
						|
reference "eq:M-approx"
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is itself a nonlinear optimization problem, so there are various methods
 | 
						|
 for approximating it, including Dogleg and Steihaug's method.
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Section
 | 
						|
Adapting the Trust Region Size
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
As mentioned in the previous section, we increase the trust region size
 | 
						|
 if the decrease in the model function 
 | 
						|
\begin_inset Formula $M$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 matches the decrease in the true cost function 
 | 
						|
\begin_inset Formula $S$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 very closely, and decrease it if they do not match closely.
 | 
						|
 The closeness of this match is measured with the 
 | 
						|
\emph on
 | 
						|
gain ratio
 | 
						|
\emph default
 | 
						|
,
 | 
						|
\begin_inset Formula 
 | 
						|
\[
 | 
						|
\rho=\frac{f\left(x\right)-f\left(x\oplus\delta x_{d}\right)}{M\left(0\right)-M\left(\delta x_{d}\right)}\text{,}
 | 
						|
\]
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
where 
 | 
						|
\begin_inset Formula $\delta x_{d}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is the proposed dogleg step to be introduced next.
 | 
						|
 The decrease in the model function is always non-negative, and as the decrease
 | 
						|
 in 
 | 
						|
\begin_inset Formula $f$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 approaches it, 
 | 
						|
\begin_inset Formula $\rho$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 approaches 
 | 
						|
\begin_inset Formula $1$
 | 
						|
\end_inset
 | 
						|
 | 
						|
.
 | 
						|
 If the true cost function increases, 
 | 
						|
\begin_inset Formula $\rho$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 will be negative, and if the true cost function decreases even more than
 | 
						|
 predicted by 
 | 
						|
\begin_inset Formula $M$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, then 
 | 
						|
\begin_inset Formula $\rho$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 will be greater than 
 | 
						|
\begin_inset Formula $1$
 | 
						|
\end_inset
 | 
						|
 | 
						|
.
 | 
						|
 A typical update rule [
 | 
						|
\color blue
 | 
						|
see where this came from in paper
 | 
						|
\color inherit
 | 
						|
] is
 | 
						|
\begin_inset Formula 
 | 
						|
\[
 | 
						|
\Delta\leftarrow\begin{cases}
 | 
						|
\max\left(\Delta,3\norm{\delta x_{d}}\right)\text{,} & \rho>0.75\\
 | 
						|
\Delta & 0.75>\rho>0.25\\
 | 
						|
\Delta/2 & 0.25>\rho
 | 
						|
\end{cases}
 | 
						|
\]
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Section
 | 
						|
Dogleg
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
Dogleg minimizes an approximation of Eq.
 | 
						|
\begin_inset space ~
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\begin_inset CommandInset ref
 | 
						|
LatexCommand ref
 | 
						|
reference "eq:M-approx"
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 by considering three possibilities using two points - the minimizer 
 | 
						|
\begin_inset Formula $\delta x_{u}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 of 
 | 
						|
\begin_inset Formula $M^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 along the negative gradient direction 
 | 
						|
\begin_inset Formula $-g^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, and the overall Newton's method minimizer 
 | 
						|
\begin_inset Formula $\delta x_{n}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 of 
 | 
						|
\begin_inset Formula $M^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
.
 | 
						|
 When the Hessian 
 | 
						|
\begin_inset Formula $G^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is positive, the magnitude of 
 | 
						|
\begin_inset Formula $\delta x_{u}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is always less than that of 
 | 
						|
\begin_inset Formula $\delta x_{n}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, meaning that the Newton's method step is 
 | 
						|
\begin_inset Quotes eld
 | 
						|
\end_inset
 | 
						|
 | 
						|
more adventurous
 | 
						|
\begin_inset Quotes erd
 | 
						|
\end_inset
 | 
						|
 | 
						|
.
 | 
						|
 How much we step towards the Newton's method point depends on the trust
 | 
						|
 region size:
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Enumerate
 | 
						|
If the trust region is smaller than 
 | 
						|
\begin_inset Formula $\delta x_{u}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, we step in the negative gradient direction but only by the trust radius.
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Enumerate
 | 
						|
If the trust region boundary is between 
 | 
						|
\begin_inset Formula $\delta x_{u}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 and 
 | 
						|
\begin_inset Formula $\delta x_{n}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, we step to the linearly-interpolated point between these two points that
 | 
						|
 intersects the trust region boundary.
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Enumerate
 | 
						|
If the trust region boundary is larger than 
 | 
						|
\begin_inset Formula $\delta x_{n}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, we step to 
 | 
						|
\begin_inset Formula $\delta x_{n}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
.
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
To find the intersection of the line between 
 | 
						|
\begin_inset Formula $\delta x_{u}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 and 
 | 
						|
\begin_inset Formula $\delta x_{n}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 with the trust region boundary, we solve a quadratic roots problem,
 | 
						|
\begin_inset Formula 
 | 
						|
\begin{align*}
 | 
						|
\Delta & =\norm{\left(1-\tau\right)\delta x_{u}+\tau\delta x_{n}}\\
 | 
						|
\Delta^{2} & =\left(1-\tau\right)^{2}\delta x_{u}^{\t}\delta x_{u}+2\tau\left(1-\tau\right)\delta x_{u}^{\t}\delta x_{n}+\tau^{2}\delta x_{n}^{\t}\delta x_{n}\\
 | 
						|
0 & =uu-2\tau uu+\tau^{2}uu+2\tau un-2\tau^{2}un+\tau^{2}nn-\Delta^{2}\\
 | 
						|
0 & =\left(uu-2un+nn\right)\tau^{2}+\left(2un-2uu\right)\tau-\Delta^{2}+uu\\
 | 
						|
\tau & =\frac{-\left(2un-2uu\right)\pm\sqrt{\left(2un-2uu\right)^{2}-4\left(uu-2un+nn\right)\left(uu-\Delta^{2}\right)}}{2\left(uu-un+nn\right)}
 | 
						|
\end{align*}
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
From this we take whichever possibility for 
 | 
						|
\begin_inset Formula $\tau$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 such that 
 | 
						|
\begin_inset Formula $0<\tau<1$
 | 
						|
\end_inset
 | 
						|
 | 
						|
.
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
To find the steepest-descent minimizer 
 | 
						|
\begin_inset Formula $\delta x_{u}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, we perform line search in the gradient direction on the approximate function
 | 
						|
 
 | 
						|
\begin_inset Formula $M$
 | 
						|
\end_inset
 | 
						|
 | 
						|
,
 | 
						|
\begin_inset Formula 
 | 
						|
\begin{equation}
 | 
						|
\delta x_{u}^{\left(k\right)}=\frac{-g^{\left(k\right)\t}g^{\left(k\right)}}{g^{\left(k\right)\t}G^{\left(k\right)}g^{\left(k\right)}}g^{\left(k\right)}\label{eq:steepest-descent-point}
 | 
						|
\end{equation}
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
Thus, mathematically, we can write the dogleg update 
 | 
						|
\begin_inset Formula $\delta x_{d}^{\left(k\right)}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 as
 | 
						|
\begin_inset Formula 
 | 
						|
\[
 | 
						|
\delta x_{d}^{\left(k\right)}=\begin{cases}
 | 
						|
-\frac{\Delta}{\norm{g^{\left(k\right)}}}g^{\left(k\right)}\text{,} & \Delta<\norm{\delta x_{u}^{\left(k\right)}}\\
 | 
						|
\left(1-\tau^{\left(k\right)}\right)\delta x_{u}^{\left(k\right)}+\tau^{\left(k\right)}\delta x_{n}^{\left(k\right)}\text{,} & \norm{\delta x_{u}^{\left(k\right)}}<\Delta<\norm{\delta x_{n}^{\left(k\right)}}\\
 | 
						|
\delta x_{n}^{\left(k\right)}\text{,} & \norm{\delta x_{n}^{\left(k\right)}}<\Delta
 | 
						|
\end{cases}
 | 
						|
\]
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Section
 | 
						|
Working with 
 | 
						|
\begin_inset Formula $M$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 as a Bayes' Net
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
When we have already eliminated a factor graph into a Bayes' Net, we have
 | 
						|
 the same quadratic error function 
 | 
						|
\begin_inset Formula $M^{\left(k\right)}\left(\delta x\right)$
 | 
						|
\end_inset
 | 
						|
 | 
						|
, but it is in a different form:
 | 
						|
\begin_inset Formula 
 | 
						|
\[
 | 
						|
M^{\left(k\right)}\left(\delta x\right)=\frac{1}{2}\norm{Rx-d}^{2}\text{,}
 | 
						|
\]
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
where 
 | 
						|
\begin_inset Formula $R$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is an upper-triangular matrix (stored as a set of sparse block Gaussian
 | 
						|
 conditionals in GTSAM), and 
 | 
						|
\begin_inset Formula $d$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 is the r.h.s.
 | 
						|
 vector.
 | 
						|
 The gradient and Hessian of 
 | 
						|
\begin_inset Formula $M$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 are then
 | 
						|
\begin_inset Formula 
 | 
						|
\begin{align*}
 | 
						|
g^{\left(k\right)} & =R^{\t}\left(Rx-d\right)\\
 | 
						|
G^{\left(k\right)} & =R^{\t}R
 | 
						|
\end{align*}
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
In GTSAM, because the Bayes' Net is not dense, we evaluate Eq.
 | 
						|
\begin_inset space ~
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\begin_inset CommandInset ref
 | 
						|
LatexCommand ref
 | 
						|
reference "eq:steepest-descent-point"
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 in an efficient way.
 | 
						|
 Rewriting the denominator (leaving out the 
 | 
						|
\begin_inset Formula $\left(k\right)$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 superscript) as
 | 
						|
\begin_inset Formula 
 | 
						|
\[
 | 
						|
g^{\t}Gg=\sum_{i}\left(R_{i}g\right)^{\t}\left(R_{i}g\right)\text{,}
 | 
						|
\]
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
where 
 | 
						|
\begin_inset Formula $i$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 indexes over the conditionals in the Bayes' Net (corresponding to blocks
 | 
						|
 of rows of 
 | 
						|
\begin_inset Formula $R$
 | 
						|
\end_inset
 | 
						|
 | 
						|
) exploits the sparse structure of the Bayes' Net, because it is easy to
 | 
						|
 only include the variables involved in each 
 | 
						|
\begin_inset Formula $i^{\text{th}}$
 | 
						|
\end_inset
 | 
						|
 | 
						|
 conditional when multiplying them by the corresponding elements of 
 | 
						|
\begin_inset Formula $g$
 | 
						|
\end_inset
 | 
						|
 | 
						|
.
 | 
						|
\end_layout
 | 
						|
 | 
						|
\begin_layout Standard
 | 
						|
\begin_inset CommandInset bibtex
 | 
						|
LatexCommand bibtex
 | 
						|
bibfiles "trustregion"
 | 
						|
options "plain"
 | 
						|
 | 
						|
\end_inset
 | 
						|
 | 
						|
 | 
						|
\end_layout
 | 
						|
 | 
						|
\end_body
 | 
						|
\end_document
 |