Merge remote-tracking branch 'svn/trunk' into windows
Conflicts: gtsam.h wrap/CMakeLists.txtrelease/4.3a0
Richard Roberts
commit
90578e2532
68
.cproject
68
.cproject
|
|
@ -761,6 +761,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testDiscreteMarginals.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testDiscreteMarginals.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1653,10 +1661,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="PlanarSLAMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="PlanarSLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>PlanarSLAMExample.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>PlanarSLAMExample_easy.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1685,10 +1693,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="PlanarSLAMExample_selfcontained.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="PlanarSLAMSelfContained_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>PlanarSLAMExample_selfcontained.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>PlanarSLAMSelfContained_advanced.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1701,18 +1709,18 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="Pose2SLAMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="Pose2SLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>Pose2SLAMExample.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>Pose2SLAMExample_easy.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="Pose2SLAMwSPCG.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="Pose2SLAMwSPCG_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>Pose2SLAMwSPCG.run</buildTarget>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>Pose2SLAMwSPCG_easy.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1741,10 +1749,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="Pose2SLAMExample_graph.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="Pose2SLAMwSPCG_advanced.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>Pose2SLAMExample_graph.run</buildTarget>
|
||||
<buildTarget>Pose2SLAMwSPCG_advanced.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2152,6 +2160,38 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap_gtsam_build" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap_gtsam_build</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap_gtsam_unstable_build" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap_gtsam_unstable_build</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap_gtsam_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap_gtsam_unstable</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>wrap</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
|
@ -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
|
||||
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 1.5 KiB |
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
After Width: | Height: | Size: 1.4 KiB |
|
|
@ -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
|
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
|
|
@ -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<IterativeOptimizationParameters>();
|
||||
|
||||
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 ;
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/nonlinear/NonlinearISAM.h>
|
||||
#include <gtsam/slam/visualSLAM.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#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; j<data.landmarks.size(); ++j)
|
||||
initials.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
|
||||
for (size_t j=0; j<data.points.size(); ++j)
|
||||
initials.insert(L(j), data.points[j] + Point3(data.noiseL->sample()));
|
||||
|
||||
// Update ISAM the first time and obtain the current estimate
|
||||
isam.update(newFactors, initials);
|
||||
|
|
@ -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<gtsam::Pose3> poses; // ground-truth camera poses
|
||||
gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM)
|
||||
std::vector<gtsam::Point3> landmarks; // ground-truth landmarks
|
||||
std::vector<gtsam::Point3> points; // ground-truth landmarks
|
||||
std::map<int, vector<gtsam::Point2> > 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; i<n; ++i) {
|
||||
theta += 2*M_PI/n;
|
||||
for (int i=0; i<n; ++i, theta += 2*M_PI/n) {
|
||||
data.poses.push_back(gtsam::Pose3(
|
||||
gtsam::Rot3(-sin(theta), 0.0, -cos(theta),
|
||||
cos(theta), 0.0, -sin(theta),
|
||||
|
|
@ -78,12 +77,12 @@ struct VisualSLAMExampleData {
|
|||
// Simulated measurements with Gaussian noise
|
||||
data.noiseZ = gtsam::sharedSigma(2, 1.0);
|
||||
for (size_t i=0; i<data.poses.size(); ++i) {
|
||||
for (size_t j=0; j<data.landmarks.size(); ++j) {
|
||||
for (size_t j=0; j<data.points.size(); ++j) {
|
||||
gtsam::SimpleCamera camera(data.poses[i], *data.sK);
|
||||
data.z[i].push_back(camera.project(data.landmarks[j]) + gtsam::Point2(data.noiseZ->sample()));
|
||||
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;
|
||||
|
|
@ -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 <gtsam/nonlinear/Symbol.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#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; i<data.poses.size(); ++i) {
|
||||
for (size_t j=0; j<data.landmarks.size(); ++j)
|
||||
for (size_t j=0; j<data.points.size(); ++j)
|
||||
graph.addMeasurement(data.z[i][j], data.noiseZ, X(i), L(j), data.sK);
|
||||
}
|
||||
// 2b. Prior factor for the first pose to resolve ambiguity (not needed for LevenbergMarquardtOptimizer)
|
||||
// 2b. Prior factor for the first pose and point to constraint the system
|
||||
graph.addPosePrior(X(0), data.poses[0], data.noiseX);
|
||||
graph.addPointPrior(L(0), data.points[0], data.noiseL);
|
||||
|
||||
/* 3. Initial estimates for variable nodes, simulated by Gaussian noises */
|
||||
Values initial;
|
||||
for (size_t i=0; i<data.poses.size(); ++i)
|
||||
initial.insert(X(i), data.poses[i]*Pose3::Expmap(data.noiseX->sample()));
|
||||
for (size_t j=0; j<data.landmarks.size(); ++j)
|
||||
initial.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
|
||||
for (size_t j=0; j<data.points.size(); ++j)
|
||||
initial.insert(L(j), data.points[j] + Point3(data.noiseL->sample()));
|
||||
initial.print("Intial Estimates: ");
|
||||
|
||||
/* 4. Optimize the graph and print results */
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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})
|
||||
|
|
@ -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'));
|
||||
|
|
@ -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'));
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
57
gtsam.h
57
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 {
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/discrete/DiscreteFactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A class for computing marginals of variables in a DiscreteFactorGraph
|
||||
*/
|
||||
class DiscreteMarginals {
|
||||
|
||||
protected:
|
||||
|
||||
BayesTree<DiscreteConditional> 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 */
|
||||
|
|
@ -10,8 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* testDiscreteFactorGraph.cpp
|
||||
*
|
||||
* @file testDiscreteFactorGraph.cpp
|
||||
* @date Feb 14, 2011
|
||||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/discrete/DiscreteMarginals.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -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<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version)
|
||||
{
|
||||
ar & boost::serialization::make_nvp("Cal3_S2Stereo",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
ar & BOOST_SERIALIZATION_NVP(K_);
|
||||
ar & BOOST_SERIALIZATION_NVP(b_);
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Cal3_S2);
|
||||
}
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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]);
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -11,9 +11,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/IterativeOptimizationParameters.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -21,25 +20,23 @@ class IterativeSolver {
|
|||
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<IterativeSolver> 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_ ; }
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -42,7 +42,7 @@ std::vector<size_t> extractColSpec_(const FactorGraph<GaussianFactor>& 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<size_t> 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 */
|
||||
|
|
|
|||
|
|
@ -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_);}
|
||||
|
||||
|
|
|
|||
|
|
@ -74,7 +74,8 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(params_.getEliminationFunction()));
|
||||
}
|
||||
else if ( params_.isCG() ) {
|
||||
SimpleSPCGSolver solver(dampedSystem, *params_.iterativeParams);
|
||||
IterativeOptimizationParameters::shared_ptr params(!params_.iterativeParams ? boost::make_shared<IterativeOptimizationParameters>() : params_.iterativeParams);
|
||||
SimpleSPCGSolver solver(dampedSystem, *params);
|
||||
delta = *solver.optimize();
|
||||
}
|
||||
else {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -49,7 +49,7 @@ namespace gtsam {
|
|||
/** return keys in some random order */
|
||||
std::set<Key> 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) */
|
||||
|
|
|
|||
|
|
@ -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> ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty)
|
||||
boost::optional<IterativeOptimizationParameters::shared_ptr> iterativeParams; ///< The container for iterativeOptimization parameters.
|
||||
IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers.
|
||||
|
||||
SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -72,6 +72,9 @@ namespace visualSLAM {
|
|||
/// get a point
|
||||
Point3 point(Key j) const { return at<Point3>(j); }
|
||||
|
||||
/// check if value with specified key exists
|
||||
bool exists(Key i) const { return gtsam::Values::exists(i); }
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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 <gtsam_unstable/dynamics/PoseRTV.h>
|
||||
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 <gtsam_unstable/dynamics/imuSystem.h>
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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";
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -279,7 +279,7 @@ void verifyReturnTypes(const vector<string>& validtypes, const vector<T>& 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";
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
};
|
||||
|
|
|
|||
15
wrap/README
15
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<Vector> and unwrap<Matrix>
|
||||
- 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
|
||||
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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" ));
|
||||
|
|
|
|||
|
|
@ -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<argc; ++i)
|
||||
cerr << argv[i] << endl;
|
||||
|
|
@ -69,6 +73,6 @@ int main(int argc, const char* argv[]) {
|
|||
usage();
|
||||
}
|
||||
else
|
||||
generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argc==5 ? " " : argv[5]);
|
||||
generate_matlab_toolbox(argv[1],argv[2],argv[3],argv[4],argv[5],argc==6 ? " " : argv[6]);
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue