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>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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">
|
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j2</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
|
|
@ -1653,10 +1661,10 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>PlanarSLAMExample.run</buildTarget>
|
<buildTarget>PlanarSLAMExample_easy.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
|
@ -1685,10 +1693,10 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>PlanarSLAMExample_selfcontained.run</buildTarget>
|
<buildTarget>PlanarSLAMSelfContained_advanced.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
|
@ -1701,18 +1709,18 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>Pose2SLAMExample.run</buildTarget>
|
<buildTarget>Pose2SLAMExample_easy.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j2</buildArguments>
|
||||||
<buildTarget>Pose2SLAMwSPCG.run</buildTarget>
|
<buildTarget>Pose2SLAMwSPCG_easy.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
|
@ -1741,10 +1749,10 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
<buildTarget>Pose2SLAMExample_graph.run</buildTarget>
|
<buildTarget>Pose2SLAMwSPCG_advanced.run</buildTarget>
|
||||||
<stopOnError>true</stopOnError>
|
<stopOnError>true</stopOnError>
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
|
|
@ -2152,6 +2160,38 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</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">
|
<target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<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_MATLAB_TESTS "Enable/Disable installation of matlab tests" ON)
|
||||||
option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON)
|
option(GTSAM_INSTALL_WRAP "Enable/Disable installation of wrap utility" ON)
|
||||||
|
|
||||||
# Experimental - features disabled by default
|
# TODO: Check for matlab mex binary before handling building of binaries
|
||||||
option(GTSAM_ENABLE_BUILD_MEX_BINARIES "Enable/Disable building of matlab mex files" OFF)
|
|
||||||
|
# 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
|
# Flags for choosing default packaging tools
|
||||||
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
|
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_EXAMPLES} "Install matlab examples ")
|
||||||
print_config_flag(${GTSAM_INSTALL_MATLAB_TESTS} "Install matlab tests ")
|
print_config_flag(${GTSAM_INSTALL_MATLAB_TESTS} "Install matlab tests ")
|
||||||
print_config_flag(${GTSAM_INSTALL_WRAP} "Install wrap utility ")
|
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 "===============================================================")
|
message(STATUS "===============================================================")
|
||||||
|
|
||||||
# Include CPack *after* all flags
|
# 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
|
// 4. Single Step Optimization using Levenberg-Marquardt
|
||||||
// Note: Although there are many options in IterativeOptimizationParameters,
|
// Note: Although there are many options in IterativeOptimizationParameters,
|
||||||
// the SimpleSPCGSolver doesn't actually use all of them at this moment.
|
Values result = graph.optimizeSPCG(initialEstimate);
|
||||||
// More detail in the next release.
|
result.print("\nFinal result:\n");
|
||||||
LevenbergMarquardtParams param;
|
|
||||||
param.linearSolverType = SuccessiveLinearizationParams::CG;
|
|
||||||
param.iterativeParams = boost::make_shared<IterativeOptimizationParameters>();
|
|
||||||
|
|
||||||
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, param);
|
|
||||||
Values result = optimizer.optimize();
|
|
||||||
cout << "final error = " << graph.error(result) << endl;
|
cout << "final error = " << graph.error(result) << endl;
|
||||||
|
|
||||||
return 0 ;
|
return 0 ;
|
||||||
|
|
|
||||||
|
|
@ -10,7 +10,7 @@
|
||||||
* -------------------------------------------------------------------------- */
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file VisualSLAMwISAM2Example.cpp
|
* @file VisualISAMExample.cpp
|
||||||
* @brief An ISAM example for synthesis sequence, single camera
|
* @brief An ISAM example for synthesis sequence, single camera
|
||||||
* @author Duy-Nguyen Ta
|
* @author Duy-Nguyen Ta
|
||||||
*/
|
*/
|
||||||
|
|
@ -19,7 +19,7 @@
|
||||||
#include <gtsam/nonlinear/NonlinearISAM.h>
|
#include <gtsam/nonlinear/NonlinearISAM.h>
|
||||||
#include <gtsam/slam/visualSLAM.h>
|
#include <gtsam/slam/visualSLAM.h>
|
||||||
#include <gtsam/slam/BetweenFactor.h>
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
#include "VisualSLAMExampleData.h"
|
#include "VisualSLAMData.h"
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
@ -72,8 +72,8 @@ int main(int argc, char* argv[]) {
|
||||||
initials.insert(X(1), pose0Init*odoMeasurement);
|
initials.insert(X(1), pose0Init*odoMeasurement);
|
||||||
|
|
||||||
// Initial values for the landmarks, simulated with Gaussian noise
|
// Initial values for the landmarks, simulated with Gaussian noise
|
||||||
for (size_t j=0; j<data.landmarks.size(); ++j)
|
for (size_t j=0; j<data.points.size(); ++j)
|
||||||
initials.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
|
initials.insert(L(j), data.points[j] + Point3(data.noiseL->sample()));
|
||||||
|
|
||||||
// Update ISAM the first time and obtain the current estimate
|
// Update ISAM the first time and obtain the current estimate
|
||||||
isam.update(newFactors, initials);
|
isam.update(newFactors, initials);
|
||||||
|
|
@ -10,8 +10,8 @@
|
||||||
* -------------------------------------------------------------------------- */
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file VisualSLAMSimulatedData.cpp
|
* @file VisualSLAMData.cpp
|
||||||
* @brief Generate ground-truth simulated data for VisualSLAM examples (SFM and ISAM2)
|
* @brief Generate ground-truth simulated data for VisualSLAM examples
|
||||||
* @author Duy-Nguyen Ta
|
* @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)
|
* - 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)
|
* (10,10,-10) (-10,10,-10) (-10,-10,-10) (10,-10,-10)
|
||||||
* - n 90-deg-FoV cameras with the same calibration parameters:
|
* - n 90-deg-FoV cameras with the same calibration parameters:
|
||||||
|
|
@ -40,7 +40,7 @@ struct VisualSLAMExampleData {
|
||||||
gtsam::shared_ptrK sK; // camera calibration parameters
|
gtsam::shared_ptrK sK; // camera calibration parameters
|
||||||
std::vector<gtsam::Pose3> poses; // ground-truth camera poses
|
std::vector<gtsam::Pose3> poses; // ground-truth camera poses
|
||||||
gtsam::Pose3 odometry; // ground-truth odometry between 2 consecutive poses (simulated data for iSAM)
|
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
|
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 noiseZ; // measurement noise (noiseModel::Isotropic::Sigma(2, 5.0f));
|
||||||
gtsam::SharedDiagonal noiseX; // noise for camera poses
|
gtsam::SharedDiagonal noiseX; // noise for camera poses
|
||||||
|
|
@ -49,14 +49,14 @@ struct VisualSLAMExampleData {
|
||||||
static const VisualSLAMExampleData generate() {
|
static const VisualSLAMExampleData generate() {
|
||||||
VisualSLAMExampleData data;
|
VisualSLAMExampleData data;
|
||||||
// Landmarks (ground truth)
|
// Landmarks (ground truth)
|
||||||
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.landmarks.push_back(gtsam::Point3(-10.0,10.0,10.0));
|
data.points.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.landmarks.push_back(gtsam::Point3(10.0,-10.0,10.0));
|
data.points.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.landmarks.push_back(gtsam::Point3(-10.0,10.0,-10.0));
|
data.points.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.landmarks.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
|
// Camera calibration parameters
|
||||||
data.sK = gtsam::shared_ptrK(new gtsam::Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
|
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;
|
int n = 8;
|
||||||
double theta = 0.0;
|
double theta = 0.0;
|
||||||
double r = 30.0;
|
double r = 30.0;
|
||||||
for (int i=0; i<n; ++i) {
|
for (int i=0; i<n; ++i, theta += 2*M_PI/n) {
|
||||||
theta += 2*M_PI/n;
|
|
||||||
data.poses.push_back(gtsam::Pose3(
|
data.poses.push_back(gtsam::Pose3(
|
||||||
gtsam::Rot3(-sin(theta), 0.0, -cos(theta),
|
gtsam::Rot3(-sin(theta), 0.0, -cos(theta),
|
||||||
cos(theta), 0.0, -sin(theta),
|
cos(theta), 0.0, -sin(theta),
|
||||||
|
|
@ -78,12 +77,12 @@ struct VisualSLAMExampleData {
|
||||||
// Simulated measurements with Gaussian noise
|
// Simulated measurements with Gaussian noise
|
||||||
data.noiseZ = gtsam::sharedSigma(2, 1.0);
|
data.noiseZ = gtsam::sharedSigma(2, 1.0);
|
||||||
for (size_t i=0; i<data.poses.size(); ++i) {
|
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);
|
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);
|
data.noiseL = gtsam::sharedSigma(3, 0.1);
|
||||||
|
|
||||||
return data;
|
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
|
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
|
||||||
* @author Duy-Nguyen Ta
|
* @author Duy-Nguyen Ta
|
||||||
*/
|
*/
|
||||||
|
|
@ -19,7 +19,7 @@
|
||||||
#include <gtsam/nonlinear/Symbol.h>
|
#include <gtsam/nonlinear/Symbol.h>
|
||||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||||
#include "VisualSLAMExampleData.h"
|
#include "VisualSLAMData.h"
|
||||||
|
|
||||||
using namespace std;
|
using namespace std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
@ -39,18 +39,19 @@ int main(int argc, char* argv[]) {
|
||||||
/* 2. Add factors to the graph */
|
/* 2. Add factors to the graph */
|
||||||
// 2a. Measurement factors
|
// 2a. Measurement factors
|
||||||
for (size_t i=0; i<data.poses.size(); ++i) {
|
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);
|
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.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 */
|
/* 3. Initial estimates for variable nodes, simulated by Gaussian noises */
|
||||||
Values initial;
|
Values initial;
|
||||||
for (size_t i=0; i<data.poses.size(); ++i)
|
for (size_t i=0; i<data.poses.size(); ++i)
|
||||||
initial.insert(X(i), data.poses[i]*Pose3::Expmap(data.noiseX->sample()));
|
initial.insert(X(i), data.poses[i]*Pose3::Expmap(data.noiseX->sample()));
|
||||||
for (size_t j=0; j<data.landmarks.size(); ++j)
|
for (size_t j=0; j<data.points.size(); ++j)
|
||||||
initial.insert(L(j), data.landmarks[j] + Point3(data.noiseL->sample()));
|
initial.insert(L(j), data.points[j] + Point3(data.noiseL->sample()));
|
||||||
initial.print("Intial Estimates: ");
|
initial.print("Intial Estimates: ");
|
||||||
|
|
||||||
/* 4. Optimize the graph and print results */
|
/* 4. Optimize the graph and print results */
|
||||||
|
|
@ -43,26 +43,13 @@ initialEstimate.print(sprintf('\nInitial estimate:\n '));
|
||||||
result = graph.optimize(initialEstimate);
|
result = graph.optimize(initialEstimate);
|
||||||
result.print(sprintf('\nFinal result:\n '));
|
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
|
%% Plot Covariance Ellipses
|
||||||
hold on
|
figure(1);clf;
|
||||||
for i=1:3
|
plot(result.xs(),result.ys(),'k*-'); hold on
|
||||||
pose_i = result.pose(i);
|
marginals = graph.marginals(result);
|
||||||
covarianceEllipse([pose_i.x;pose_i.y],P{i},'g')
|
for i=1:result.size()
|
||||||
|
pose_i = result.pose(i);
|
||||||
|
P{i}=marginals.marginalCovariance(i);
|
||||||
|
plotPose2(pose_i,'g',P{i})
|
||||||
end
|
end
|
||||||
|
axis equal
|
||||||
|
|
|
||||||
|
|
@ -54,3 +54,16 @@ initialEstimate.print(sprintf('\nInitial estimate:\n'));
|
||||||
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
|
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
|
||||||
result = graph.optimize(initialEstimate);
|
result = graph.optimize(initialEstimate);
|
||||||
result.print(sprintf('\nFinal result:\n'));
|
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
|
for i=1:result.size()-1
|
||||||
pose_i = result.pose(i);
|
pose_i = result.pose(i);
|
||||||
P{i}=marginals.marginalCovariance(i);
|
P{i}=marginals.marginalCovariance(i);
|
||||||
covarianceEllipse([pose_i.x;pose_i.y],P{i},'b')
|
plotPose2(pose_i,'b',P{i})
|
||||||
end
|
end
|
||||||
fprintf(1,'%.5f %.5f %.5f\n',P{99})
|
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
|
%% Plot Initial Estimate
|
||||||
figure(1);clf
|
figure(1);clf
|
||||||
plot3(initial.xs(),initial.ys(),initial.zs(),'g-*'); axis equal
|
plot3(initial.xs(),initial.ys(),initial.zs(),'g-*');
|
||||||
|
|
||||||
%% optimize
|
%% optimize
|
||||||
result = fg.optimize(initial);
|
result = fg.optimize(initial);
|
||||||
|
|
||||||
%% Show Result
|
%% 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'));
|
result.print(sprintf('\nFinal result:\n'));
|
||||||
|
|
|
||||||
|
|
@ -16,18 +16,17 @@ N = 2500;
|
||||||
filename = '../Data/sphere2500.txt';
|
filename = '../Data/sphere2500.txt';
|
||||||
|
|
||||||
%% Initialize graph, initial estimate, and odometry noise
|
%% 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);
|
[graph,initial]=load3D(filename,model,true,N);
|
||||||
first = initial.pose(0);
|
model = gtsamSharedNoiseModel_Sigmas([0.05; 0.05; 0.05; 5*pi/180; 5*pi/180; 5*pi/180]);
|
||||||
graph.addHardConstraint(0, first);
|
|
||||||
|
|
||||||
%% Plot Initial Estimate
|
%% Plot Initial Estimate
|
||||||
figure(1);clf
|
figure(1);clf
|
||||||
|
first = initial.pose(0);
|
||||||
plot3(first.x(),first.y(),first.z(),'r*'); hold on
|
plot3(first.x(),first.y(),first.z(),'r*'); hold on
|
||||||
plot3DTrajectory(initial,'g-',false);
|
plot3DTrajectory(initial,'g-',false);
|
||||||
|
|
||||||
%% Read again, now all constraints
|
%% Read again, now with all constraints, and optimize
|
||||||
[graph,discard]=load3D(filename,model,false,N);
|
graph = load3D(filename,model,false,N);
|
||||||
graph.addHardConstraint(0, first);
|
graph.addHardConstraint(0, first);
|
||||||
result = graph.optimize(initial); % start from old result
|
result = graph.optimize(initial);
|
||||||
plot3DTrajectory(result,'r-',false); axis equal;
|
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
|
% covarianceEllipse: plot a Gaussian as an uncertainty ellipse
|
||||||
% Based on Maybeck Vol 1, page 366
|
% Based on Maybeck Vol 1, page 366
|
||||||
% k=2.296 corresponds to 1 std, 68.26% of all probability
|
% 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)
|
% covarianceEllipse(x,P,color)
|
||||||
% it is assumed x and y are the first two components of state x
|
% 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) );
|
[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
|
||||||
line(ex,ey,'color',color);
|
line(ex,ey,'color',color);
|
||||||
|
|
||||||
function [x,y] = ellipse(a,b,c);
|
function [x,y] = ellipse(a,b,c);
|
||||||
% ellipse: return the x and y coordinates for an ellipse
|
% ellipse: return the x and y coordinates for an ellipse
|
||||||
% [x,y] = ellipse(a,b,c);
|
% [x,y] = ellipse(a,b,c);
|
||||||
% a, and b are the axes. c is the center
|
% a, and b are the axes. c is the center
|
||||||
|
|
||||||
global ellipse_x ellipse_y
|
global ellipse_x ellipse_y
|
||||||
if ~exist('elipse_x')
|
if ~exist('elipse_x')
|
||||||
q =0:2*pi/25:2*pi;
|
q =0:2*pi/25:2*pi;
|
||||||
ellipse_x = cos(q);
|
ellipse_x = cos(q);
|
||||||
ellipse_y = sin(q);
|
ellipse_y = sin(q);
|
||||||
end
|
end
|
||||||
|
|
||||||
points = a*ellipse_x + b*ellipse_y;
|
points = a*ellipse_x + b*ellipse_y;
|
||||||
x = c(1) + points(1,:);
|
x = c(1) + points(1,:);
|
||||||
y = c(2) + points(2,:);
|
y = c(2) + points(2,:);
|
||||||
end
|
end
|
||||||
|
|
||||||
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
|
% plot3DTrajectory
|
||||||
if nargin<3,frames=false;end
|
if nargin<3,frames=false;end
|
||||||
|
if nargin<4,scale=0;end
|
||||||
|
|
||||||
plot3(values.xs(),values.ys(),values.zs(),style); hold on
|
plot3(values.xs(),values.ys(),values.zs(),style); hold on
|
||||||
if frames
|
if frames
|
||||||
|
N=values.size;
|
||||||
for i=0:N-1
|
for i=0:N-1
|
||||||
pose = values.pose(i);
|
pose = values.pose(i);
|
||||||
t = pose.translation;
|
t = pose.translation;
|
||||||
R = pose.rotation.matrix;
|
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,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),'g');
|
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),'b');
|
quiver3(t.x,t.y,t.z,R(1,3),R(2,3),R(3,3),scale,'b');
|
||||||
end
|
end
|
||||||
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
|
* Only one Method/Constructor per line
|
||||||
* Methods can return
|
* Methods can return
|
||||||
* - Eigen types: Matrix, Vector
|
* - 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
|
* - void
|
||||||
* - Any class with which be copied with boost::make_shared()
|
* - Any class with which be copied with boost::make_shared()
|
||||||
* - boost::shared_ptr of any object type
|
* - boost::shared_ptr of any object type
|
||||||
* Limitations on methods
|
* Limitations on methods
|
||||||
* - Parsing does not support overloading
|
* - 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
|
* Arguments to functions any of
|
||||||
* - Eigen types: Matrix, Vector
|
* - Eigen types: Matrix, Vector
|
||||||
* - Eigen types and classes as an optionally const reference
|
* - 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)
|
* - Any class with which be copied with boost::make_shared() (except Eigen)
|
||||||
* - boost::shared_ptr of any object type (except Eigen)
|
* - boost::shared_ptr of any object type (except Eigen)
|
||||||
* Comments can use either C++ or C style, with multiple lines
|
* Comments can use either C++ or C style, with multiple lines
|
||||||
|
|
@ -71,6 +71,7 @@ class Point2 {
|
||||||
// Standard Constructors
|
// Standard Constructors
|
||||||
Point2();
|
Point2();
|
||||||
Point2(double x, double y);
|
Point2(double x, double y);
|
||||||
|
Point2(Vector v);
|
||||||
|
|
||||||
// Testable
|
// Testable
|
||||||
void print(string s) const;
|
void print(string s) const;
|
||||||
|
|
@ -174,7 +175,7 @@ class Rot2 {
|
||||||
|
|
||||||
// Group
|
// Group
|
||||||
static gtsam::Rot2 identity();
|
static gtsam::Rot2 identity();
|
||||||
static gtsam::Rot2 inverse();
|
gtsam::Rot2 inverse();
|
||||||
gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
|
gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
|
||||||
gtsam::Rot2 between(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 gtsam::Pose2 Expmap(Vector v);
|
||||||
static Vector Logmap(const gtsam::Pose2& p);
|
static Vector Logmap(const gtsam::Pose2& p);
|
||||||
Matrix adjointMap() const;
|
Matrix adjointMap() const;
|
||||||
Vector adjoint() const;
|
Vector adjoint(const Vector& xi) const;
|
||||||
static Matrix wedge();
|
static Matrix wedge(double vx, double vy, double w);
|
||||||
|
|
||||||
// Group Actions on Point2
|
// Group Actions on Point2
|
||||||
gtsam::Point2 transform_from(const gtsam::Point2& p) const;
|
gtsam::Point2 transform_from(const gtsam::Point2& p) const;
|
||||||
|
|
@ -306,7 +307,7 @@ class Pose3 {
|
||||||
Pose3();
|
Pose3();
|
||||||
Pose3(const gtsam::Pose3& pose);
|
Pose3(const gtsam::Pose3& pose);
|
||||||
Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
|
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);
|
Pose3(Matrix t);
|
||||||
|
|
||||||
// Testable
|
// Testable
|
||||||
|
|
@ -344,9 +345,9 @@ class Pose3 {
|
||||||
double y() const;
|
double y() const;
|
||||||
double z() const;
|
double z() const;
|
||||||
Matrix matrix() 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::Point3& point);
|
||||||
// double range(const gtsam::Pose3& pose);
|
// double range(const gtsam::Pose3& pose); // FIXME: shadows other range
|
||||||
};
|
};
|
||||||
|
|
||||||
class Cal3_S2 {
|
class Cal3_S2 {
|
||||||
|
|
@ -389,16 +390,6 @@ class Cal3_S2Stereo {
|
||||||
void print(string s) const;
|
void print(string s) const;
|
||||||
bool equals(const gtsam::Cal3_S2Stereo& pose, double tol) 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
|
// Standard Interface
|
||||||
double fx() const;
|
double fx() const;
|
||||||
double fy() const;
|
double fy() const;
|
||||||
|
|
@ -406,9 +397,6 @@ class Cal3_S2Stereo {
|
||||||
double px() const;
|
double px() const;
|
||||||
double py() const;
|
double py() const;
|
||||||
gtsam::Point2 principalPoint() const;
|
gtsam::Point2 principalPoint() const;
|
||||||
Vector vector() const;
|
|
||||||
Matrix matrix() const;
|
|
||||||
Matrix matrix_inverse() const;
|
|
||||||
double baseline() const;
|
double baseline() const;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
@ -442,6 +430,29 @@ class CalibratedCamera {
|
||||||
double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
|
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
|
// inference
|
||||||
//*************************************************************************
|
//*************************************************************************
|
||||||
|
|
@ -694,6 +705,7 @@ class Graph {
|
||||||
void addOdometry(size_t key1, size_t key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
|
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);
|
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 optimize(const pose2SLAM::Values& initialEstimate) const;
|
||||||
|
pose2SLAM::Values optimizeSPCG(const pose2SLAM::Values& initialEstimate) const;
|
||||||
gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
|
gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
@ -864,6 +876,7 @@ class Values {
|
||||||
void print(string s) const;
|
void print(string s) const;
|
||||||
gtsam::Pose3 pose(size_t i);
|
gtsam::Pose3 pose(size_t i);
|
||||||
gtsam::Point3 point(size_t j);
|
gtsam::Point3 point(size_t j);
|
||||||
|
bool exists(size_t key);
|
||||||
};
|
};
|
||||||
|
|
||||||
class Graph {
|
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
|
* @date Feb 14, 2011
|
||||||
* @author Duy-Nguyen Ta
|
* @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
|
* @ingroup geometry
|
||||||
* \nosubgrouping
|
* \nosubgrouping
|
||||||
*/
|
*/
|
||||||
class Cal3_S2Stereo: public Cal3_S2 {
|
class Cal3_S2Stereo {
|
||||||
private:
|
private:
|
||||||
|
|
||||||
|
Cal3_S2 K_;
|
||||||
double b_;
|
double b_;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
@ -39,12 +41,12 @@ namespace gtsam {
|
||||||
|
|
||||||
/// default calibration leaves coordinates unchanged
|
/// default calibration leaves coordinates unchanged
|
||||||
Cal3_S2Stereo() :
|
Cal3_S2Stereo() :
|
||||||
Cal3_S2(1, 1, 0, 0, 0), b_(1.0) {
|
K_(1, 1, 0, 0, 0), b_(1.0) {
|
||||||
}
|
}
|
||||||
|
|
||||||
/// constructor from doubles
|
/// constructor from doubles
|
||||||
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) :
|
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 {
|
void print(const std::string& s = "") const {
|
||||||
gtsam::print(matrix(), s);
|
K_.print(s+"K: ");
|
||||||
std::cout << "Baseline: " << b_ << std::endl;
|
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
|
/// @name Standard Interface
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
//TODO: remove?
|
/// focal length x
|
||||||
// /**
|
inline double fx() const { return K_.fx();}
|
||||||
// * Check if equal up to specified tolerance
|
|
||||||
// */
|
|
||||||
// bool equals(const Cal3_S2& K, double tol = 10e-9) const;
|
|
||||||
|
|
||||||
|
/// 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_; }
|
inline double baseline() const { return b_; }
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
|
|
@ -81,10 +101,8 @@ namespace gtsam {
|
||||||
template<class Archive>
|
template<class Archive>
|
||||||
void serialize(Archive & ar, const unsigned int version)
|
void serialize(Archive & ar, const unsigned int version)
|
||||||
{
|
{
|
||||||
ar & boost::serialization::make_nvp("Cal3_S2Stereo",
|
ar & BOOST_SERIALIZATION_NVP(K_);
|
||||||
boost::serialization::base_object<Value>(*this));
|
|
||||||
ar & BOOST_SERIALIZATION_NVP(b_);
|
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
|
* 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 Point2 pn = k_.calibrate(pi);
|
||||||
const Point3 pc(pn.x()*scale, pn.y()*scale, scale);
|
const Point3 pc(pn.x()*scale, pn.y()*scale, scale);
|
||||||
return pose_.transform_from(pc);
|
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);
|
return backproject(pi, scale);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -71,16 +71,6 @@ public:
|
||||||
return project(point, H1, H2);
|
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 {
|
Point3 backproject(const StereoPoint2& z) const {
|
||||||
Vector measured = z.vector();
|
Vector measured = z.vector();
|
||||||
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);
|
double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);
|
||||||
|
|
|
||||||
|
|
@ -113,7 +113,9 @@ struct PreconditionerParameters {
|
||||||
|
|
||||||
std::cout << "PreconditionerParameters: "
|
std::cout << "PreconditionerParameters: "
|
||||||
<< "kernel = " << kernelStr[kernel_]
|
<< "kernel = " << kernelStr[kernel_]
|
||||||
<< ", type = " << typeStr[type_] << std::endl;
|
<< ", type = " << typeStr[type_]
|
||||||
|
<< ", verbosity = " << verbosity_
|
||||||
|
<< std::endl;
|
||||||
combinatorial_.print();
|
combinatorial_.print();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
@ -167,6 +169,7 @@ struct ConjugateGradientParameters {
|
||||||
<< ", eps_rel = " << epsilon_rel_
|
<< ", eps_rel = " << epsilon_rel_
|
||||||
<< ", eps_abs = " << epsilon_abs_
|
<< ", eps_abs = " << epsilon_abs_
|
||||||
<< ", degree = " << degree_
|
<< ", degree = " << degree_
|
||||||
|
<< ", verbosity = " << verbosity_
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
@ -180,8 +183,8 @@ public:
|
||||||
|
|
||||||
PreconditionerParameters preconditioner_;
|
PreconditionerParameters preconditioner_;
|
||||||
ConjugateGradientParameters cg_;
|
ConjugateGradientParameters cg_;
|
||||||
enum Kernel { PCG = 0, LSPCG } kernel_ ; /* Iterative Method Kernel */
|
enum Kernel { PCG = 0, LSPCG = 1 } kernel_ ; ///< Iterative Method Kernel
|
||||||
enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; /* Verbosity */
|
enum Verbosity { SILENT = 0, COMPLEXITY = 1, ERROR = 2} verbosity_ ; ///< Verbosity
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
|
|
@ -221,7 +224,8 @@ public:
|
||||||
const std::string kernelStr[2] = {"pcg", "lspcg"};
|
const std::string kernelStr[2] = {"pcg", "lspcg"};
|
||||||
std::cout << s << std::endl
|
std::cout << s << std::endl
|
||||||
<< "IterativeOptimizationParameters: "
|
<< "IterativeOptimizationParameters: "
|
||||||
<< "kernel = " << kernelStr[kernel_] << std::endl;
|
<< "kernel = " << kernelStr[kernel_]
|
||||||
|
<< ", verbosity = " << verbosity_ << std::endl;
|
||||||
cg_.print();
|
cg_.print();
|
||||||
preconditioner_.print();
|
preconditioner_.print();
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -11,9 +11,8 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/linear/VectorValues.h>
|
|
||||||
#include <gtsam/linear/IterativeOptimizationParameters.h>
|
#include <gtsam/linear/IterativeOptimizationParameters.h>
|
||||||
#include <boost/shared_ptr.hpp>
|
#include <gtsam/linear/VectorValues.h>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
|
|
@ -21,25 +20,23 @@ class IterativeSolver {
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
typedef boost::shared_ptr<IterativeSolver> shared_ptr;
|
|
||||||
typedef IterativeOptimizationParameters Parameters;
|
typedef IterativeOptimizationParameters Parameters;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
Parameters::shared_ptr parameters_ ;
|
Parameters parameters_ ;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
IterativeSolver(): parameters_(new Parameters()) {}
|
IterativeSolver(): parameters_() {}
|
||||||
IterativeSolver(const IterativeSolver &solver) : parameters_(solver.parameters_) {}
|
IterativeSolver(const IterativeSolver &solver) : parameters_(solver.parameters_) {}
|
||||||
IterativeSolver(const Parameters::shared_ptr& parameters) : parameters_(parameters) {}
|
IterativeSolver(const Parameters ¶meters) : parameters_(parameters) {}
|
||||||
IterativeSolver(const Parameters ¶meters) : parameters_(new Parameters(parameters)) {}
|
|
||||||
|
|
||||||
virtual ~IterativeSolver() {}
|
virtual ~IterativeSolver() {}
|
||||||
|
|
||||||
virtual VectorValues::shared_ptr optimize () = 0;
|
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;
|
return spec;
|
||||||
}
|
}
|
||||||
|
|
||||||
SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr ¶meters)
|
SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters)
|
||||||
: Base(parameters)
|
: Base(parameters)
|
||||||
{
|
{
|
||||||
std::vector<size_t> colSpec = extractColSpec_(gfg, VariableIndex(gfg));
|
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 ;
|
double gamma = s.vector().squaredNorm(), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ;
|
||||||
|
|
||||||
const double threshold =
|
const double threshold =
|
||||||
::max(parameters_->epsilon_abs(),
|
::max(parameters_.epsilon_abs(),
|
||||||
parameters_->epsilon() * parameters_->epsilon() * gamma);
|
parameters_.epsilon() * parameters_.epsilon() * gamma);
|
||||||
const size_t iMaxIterations = parameters_->maxIterations();
|
const size_t iMaxIterations = parameters_.maxIterations();
|
||||||
|
const ConjugateGradientParameters::Verbosity verbosity = parameters_.cg_.verbosity();
|
||||||
|
|
||||||
if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR )
|
if ( verbosity >= ConjugateGradientParameters::ERROR )
|
||||||
cout << "[SimpleSPCGSolver] epsilon = " << parameters_->epsilon()
|
cout << "[SimpleSPCGSolver] epsilon = " << parameters_.epsilon()
|
||||||
<< ", max = " << parameters_->maxIterations()
|
<< ", max = " << parameters_.maxIterations()
|
||||||
<< ", ||r0|| = " << std::sqrt(gamma)
|
<< ", ||r0|| = " << std::sqrt(gamma)
|
||||||
<< ", threshold = " << threshold << std::endl;
|
<< ", threshold = " << threshold << std::endl;
|
||||||
|
|
||||||
|
|
@ -120,14 +121,14 @@ VectorValues::shared_ptr SimpleSPCGSolver::optimize (const VectorValues &initial
|
||||||
p.vector() = s.vector() + beta * p.vector();
|
p.vector() = s.vector() + beta * p.vector();
|
||||||
gamma = new_gamma ;
|
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;
|
cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ( gamma < threshold ) break ;
|
if ( gamma < threshold ) break ;
|
||||||
} // k
|
} // k
|
||||||
|
|
||||||
if ( parameters_->verbosity() >= IterativeOptimizationParameters::ERROR )
|
if ( verbosity >= ConjugateGradientParameters::ERROR )
|
||||||
cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl;
|
cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << endl;
|
||||||
|
|
||||||
/* transform y back to x */
|
/* transform y back to x */
|
||||||
|
|
|
||||||
|
|
@ -55,7 +55,7 @@ protected:
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters::shared_ptr ¶meters);
|
SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters ¶meters);
|
||||||
virtual ~SimpleSPCGSolver() {}
|
virtual ~SimpleSPCGSolver() {}
|
||||||
virtual VectorValues::shared_ptr optimize () {return optimize(*y0_);}
|
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()));
|
delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(params_.getEliminationFunction()));
|
||||||
}
|
}
|
||||||
else if ( params_.isCG() ) {
|
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();
|
delta = *solver.optimize();
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
|
|
||||||
|
|
@ -118,7 +118,7 @@ public:
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calculate the error of the factor
|
* 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.
|
* You can override this for systems with unusual noise models.
|
||||||
*/
|
*/
|
||||||
virtual double error(const Values& c) const = 0;
|
virtual double error(const Values& c) const = 0;
|
||||||
|
|
|
||||||
|
|
@ -49,7 +49,7 @@ namespace gtsam {
|
||||||
/** return keys in some random order */
|
/** return keys in some random order */
|
||||||
std::set<Key> keys() const;
|
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;
|
double error(const Values& c) const;
|
||||||
|
|
||||||
/** Unnormalized probability. O(n) */
|
/** Unnormalized probability. O(n) */
|
||||||
|
|
|
||||||
|
|
@ -31,13 +31,13 @@ public:
|
||||||
MULTIFRONTAL_QR,
|
MULTIFRONTAL_QR,
|
||||||
SEQUENTIAL_CHOLESKY,
|
SEQUENTIAL_CHOLESKY,
|
||||||
SEQUENTIAL_QR,
|
SEQUENTIAL_QR,
|
||||||
CHOLMOD, /* Experimental Flag */
|
|
||||||
CG, /* Experimental Flag */
|
CG, /* Experimental Flag */
|
||||||
|
CHOLMOD, /* Experimental Flag */
|
||||||
};
|
};
|
||||||
|
|
||||||
LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
|
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<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) {}
|
SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -85,6 +85,13 @@ namespace pose2SLAM {
|
||||||
return LevenbergMarquardtOptimizer(*this, initialEstimate).optimize();
|
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
|
} // pose2SLAM
|
||||||
|
|
|
||||||
|
|
@ -107,6 +107,7 @@ namespace pose2SLAM {
|
||||||
|
|
||||||
/// Optimize
|
/// Optimize
|
||||||
Values optimize(const Values& initialEstimate) const;
|
Values optimize(const Values& initialEstimate) const;
|
||||||
|
Values optimizeSPCG(const Values& initialEstimate) const;
|
||||||
|
|
||||||
/// Return a Marginals object
|
/// Return a Marginals object
|
||||||
Marginals marginals(const Values& solution) const {
|
Marginals marginals(const Values& solution) const {
|
||||||
|
|
|
||||||
|
|
@ -72,6 +72,9 @@ namespace visualSLAM {
|
||||||
/// get a point
|
/// get a point
|
||||||
Point3 point(Key j) const { return at<Point3>(j); }
|
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)
|
include(GtsamMatlabWrap)
|
||||||
|
|
||||||
# Wrap codegen
|
# 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
|
# mexExtension : OS/CPU-dependent extension for MEX binaries
|
||||||
# interfacePath : *absolute* path to directory of module interface file
|
# interfacePath : *absolute* path to directory of module interface file
|
||||||
# moduleName : the name of the module, interface file must be called moduleName.h
|
# moduleName : the name of the module, interface file must be called moduleName.h
|
||||||
|
|
@ -87,18 +88,29 @@ if (GTSAM_BUILD_WRAP)
|
||||||
|
|
||||||
# Code generation command
|
# Code generation command
|
||||||
add_custom_target(wrap_gtsam_unstable ALL 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)
|
DEPENDS wrap)
|
||||||
|
|
||||||
# Build command
|
# Build command
|
||||||
# Experimental: requires matlab to be on your path
|
|
||||||
if (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
if (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
||||||
# Actually compile the mex files when building the library
|
# Actually compile the mex files when building the library
|
||||||
set(TOOLBOX_MAKE_FLAGS "-j2")
|
if (GTSAM_ENABLE_BUILD_MEX_BINARIES_ALL)
|
||||||
add_custom_target(wrap_gtsam_unstable_build
|
add_custom_target(wrap_gtsam_unstable_build ALL
|
||||||
COMMAND make ${TOOLBOX_MAKE_FLAGS}
|
COMMAND make ${GTSAM_BUILD_MEX_BINARY_FLAGS}
|
||||||
WORKING_DIRECTORY ${toolbox_path}
|
WORKING_DIRECTORY ${toolbox_path}
|
||||||
DEPENDS wrap_gtsam_unstable)
|
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)
|
endif (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
||||||
|
|
||||||
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
||||||
|
|
|
||||||
|
|
@ -2,15 +2,14 @@
|
||||||
* Matlab toolbox interface definition for gtsam_unstable
|
* 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
|
// specify the classes from gtsam we are using
|
||||||
class gtsam::Point3;
|
class gtsam::Point3;
|
||||||
class gtsam::Rot3;
|
class gtsam::Rot3;
|
||||||
class gtsam::Pose3;
|
class gtsam::Pose3;
|
||||||
class gtsam::SharedNoiseModel;
|
class gtsam::SharedNoiseModel;
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
#include <gtsam_unstable/dynamics/PoseRTV.h>
|
#include <gtsam_unstable/dynamics/PoseRTV.h>
|
||||||
class PoseRTV {
|
class PoseRTV {
|
||||||
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);
|
PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);
|
||||||
|
|
||||||
// testable
|
// testable
|
||||||
bool equals(const PoseRTV& other, double tol) const;
|
bool equals(const gtsam::PoseRTV& other, double tol) const;
|
||||||
void print(string s) const;
|
void print(string s) const;
|
||||||
|
|
||||||
// access
|
// access
|
||||||
|
|
@ -39,27 +38,29 @@ class PoseRTV {
|
||||||
// manifold/Lie
|
// manifold/Lie
|
||||||
static size_t Dim();
|
static size_t Dim();
|
||||||
size_t dim() const;
|
size_t dim() const;
|
||||||
PoseRTV retract(Vector v) const;
|
gtsam::PoseRTV retract(Vector v) const;
|
||||||
Vector localCoordinates(const PoseRTV& p) const;
|
Vector localCoordinates(const gtsam::PoseRTV& p) const;
|
||||||
static PoseRTV Expmap(Vector v);
|
static gtsam::PoseRTV Expmap(Vector v);
|
||||||
static Vector Logmap(const PoseRTV& p);
|
static Vector Logmap(const gtsam::PoseRTV& p);
|
||||||
PoseRTV inverse() const;
|
gtsam::PoseRTV inverse() const;
|
||||||
PoseRTV compose(const PoseRTV& p) const;
|
gtsam::PoseRTV compose(const gtsam::PoseRTV& p) const;
|
||||||
PoseRTV between(const PoseRTV& p) const;
|
gtsam::PoseRTV between(const gtsam::PoseRTV& p) const;
|
||||||
|
|
||||||
// measurement
|
// measurement
|
||||||
double range(const PoseRTV& other) const;
|
double range(const gtsam::PoseRTV& other) const;
|
||||||
PoseRTV transformed_from(const gtsam::Pose3& trans) const;
|
gtsam::PoseRTV transformed_from(const gtsam::Pose3& trans) const;
|
||||||
|
|
||||||
// IMU/dynamics
|
// IMU/dynamics
|
||||||
PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const;
|
gtsam::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;
|
gtsam::PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const;
|
||||||
PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const;
|
gtsam::PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const;
|
||||||
Vector imuPrediction(const PoseRTV& x2, double dt) const;
|
Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const;
|
||||||
gtsam::Point3 translationIntegration(const PoseRTV& x2, double dt) const;
|
gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const;
|
||||||
Vector translationIntegrationVec(const PoseRTV& x2, double dt) const;
|
Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
}///\namespace gtsam
|
||||||
|
|
||||||
#include <gtsam_unstable/dynamics/imuSystem.h>
|
#include <gtsam_unstable/dynamics/imuSystem.h>
|
||||||
namespace imu {
|
namespace imu {
|
||||||
|
|
||||||
|
|
@ -67,8 +68,8 @@ class Values {
|
||||||
Values();
|
Values();
|
||||||
void print(string s) const;
|
void print(string s) const;
|
||||||
|
|
||||||
void insertPose(size_t key, const PoseRTV& pose);
|
void insertPose(size_t key, const gtsam::PoseRTV& pose);
|
||||||
PoseRTV pose(size_t key) const;
|
gtsam::PoseRTV pose(size_t key) const;
|
||||||
};
|
};
|
||||||
|
|
||||||
class Graph {
|
class Graph {
|
||||||
|
|
@ -76,8 +77,8 @@ class Graph {
|
||||||
void print(string s) const;
|
void print(string s) const;
|
||||||
|
|
||||||
// prior factors
|
// prior factors
|
||||||
void addPrior(size_t key, const PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel);
|
void addPrior(size_t key, const gtsam::PoseRTV& pose, const gtsam::SharedNoiseModel& noiseModel);
|
||||||
void addConstraint(size_t key, const PoseRTV& pose);
|
void addConstraint(size_t key, const gtsam::PoseRTV& pose);
|
||||||
void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel);
|
void addHeightPrior(size_t key, double z, const gtsam::SharedNoiseModel& noiseModel);
|
||||||
|
|
||||||
// inertial factors
|
// inertial factors
|
||||||
|
|
@ -86,7 +87,7 @@ class Graph {
|
||||||
void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel);
|
void addVelocityConstraint(size_t key1, size_t key2, double dt, const gtsam::SharedNoiseModel& noiseModel);
|
||||||
|
|
||||||
// other measurements
|
// 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);
|
void addRange(size_t key1, size_t key2, double z, const gtsam::SharedNoiseModel& noiseModel);
|
||||||
|
|
||||||
// optimization
|
// optimization
|
||||||
|
|
|
||||||
|
|
@ -30,7 +30,7 @@ string Argument::matlabClass(const string& delim) const {
|
||||||
string result;
|
string result;
|
||||||
BOOST_FOREACH(const string& ns, namespaces)
|
BOOST_FOREACH(const string& ns, namespaces)
|
||||||
result += ns + delim;
|
result += ns + delim;
|
||||||
if (type=="string")
|
if (type=="string" || type=="unsigned char" || type=="char")
|
||||||
return result + "char";
|
return result + "char";
|
||||||
if (type=="bool" || type=="int" || type=="size_t" || type=="Vector" || type=="Matrix")
|
if (type=="bool" || type=="int" || type=="size_t" || type=="Vector" || type=="Matrix")
|
||||||
return result + "double";
|
return result + "double";
|
||||||
|
|
|
||||||
|
|
@ -23,14 +23,15 @@ if (GTSAM_BUILD_TESTS)
|
||||||
endif(GTSAM_BUILD_TESTS)
|
endif(GTSAM_BUILD_TESTS)
|
||||||
|
|
||||||
# Wrap codegen
|
# 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
|
# mexExtension : OS/CPU-dependent extension for MEX binaries
|
||||||
# interfacePath : *absolute* path to directory of module interface file
|
# interfacePath : *absolute* path to directory of module interface file
|
||||||
# moduleName : the name of the module, interface file must be called moduleName.h
|
# moduleName : the name of the module, interface file must be called moduleName.h
|
||||||
# toolboxPath : the directory in which to generate the wrappers
|
# toolboxPath : the directory in which to generate the wrappers
|
||||||
# [mexFlags] : extra flags for the mex command
|
# [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)
|
if(MSVC OR CYGWIN OR WINGW)
|
||||||
set(mexFlags "${mexFlags} LINKFLAGS='$LINKFLAGS /LIBPATH:${Boost_LIBRARY_DIRS}'")
|
set(mexFlags "${mexFlags} LINKFLAGS='$LINKFLAGS /LIBPATH:${Boost_LIBRARY_DIRS}'")
|
||||||
endif()
|
endif()
|
||||||
|
|
@ -48,24 +49,39 @@ endif()
|
||||||
# Code generation command
|
# Code generation command
|
||||||
get_property(WRAP_EXE TARGET wrap PROPERTY LOCATION)
|
get_property(WRAP_EXE TARGET wrap PROPERTY LOCATION)
|
||||||
add_custom_target(wrap_gtsam ALL COMMAND
|
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)
|
DEPENDS wrap)
|
||||||
|
|
||||||
# Build command
|
# Build command
|
||||||
# Experimental: requires matlab to be on your path
|
|
||||||
if (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
if (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
||||||
# Actually compile the mex files when building the library
|
# Actually compile the mex files when building the library
|
||||||
set(TOOLBOX_MAKE_FLAGS "-j2")
|
# TODO: pass correct make flags from parent process
|
||||||
add_custom_target(wrap_gtsam_build
|
message(STATUS "Building Matlab MEX binaries for toolbox with flags ${GTSAM_BUILD_MEX_BINARY_FLAGS}")
|
||||||
COMMAND make ${TOOLBOX_MAKE_FLAGS}
|
if (GTSAM_ENABLE_BUILD_MEX_BINARIES_ALL)
|
||||||
WORKING_DIRECTORY ${toolbox_path}
|
add_custom_target(wrap_gtsam_build ALL
|
||||||
DEPENDS wrap_gtsam)
|
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)
|
endif (GTSAM_ENABLE_BUILD_MEX_BINARIES)
|
||||||
|
|
||||||
set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox")
|
set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox")
|
||||||
|
|
||||||
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
|
||||||
# Primary toolbox files
|
# 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}")
|
message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}")
|
||||||
install(DIRECTORY DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}) # make an empty folder
|
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
|
# 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)
|
install(FILES ${matlab_examples} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/examples)
|
||||||
|
|
||||||
message(STATUS "Installing Matlab Toolbox Examples (Data)")
|
message(STATUS "Installing Matlab Toolbox Examples (Data)")
|
||||||
set(data_excludes "${CMAKE_SOURCE_DIR}/examples/Data/.svn")
|
# Data files: *.graph and *.txt
|
||||||
file(GLOB matlab_examples_data "${CMAKE_SOURCE_DIR}/examples/Data/*.*")
|
file(GLOB matlab_examples_data_graph "${CMAKE_SOURCE_DIR}/examples/Data/*.graph")
|
||||||
list(REMOVE_ITEM matlab_examples_data ${data_excludes})
|
file(GLOB matlab_examples_data_txt "${CMAKE_SOURCE_DIR}/examples/Data/*.txt")
|
||||||
install(FILES ${matlab_examples_data} DESTINATION ${GTSAM_TOOLBOX_INSTALL_PATH}/gtsam/examples/Data)
|
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)
|
endif (GTSAM_INSTALL_MATLAB_EXAMPLES)
|
||||||
|
|
||||||
# Tests
|
# 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 {
|
const string& mexExt, const string& mexFlags) const {
|
||||||
|
|
||||||
fs::create_directories(toolboxPath);
|
fs::create_directories(toolboxPath);
|
||||||
|
|
@ -299,7 +299,7 @@ void Module::matlab_code(const string& toolboxPath,
|
||||||
makeModuleMfile.oss << "clear delims" << endl;
|
makeModuleMfile.oss << "clear delims" << endl;
|
||||||
makeModuleMfile.oss << "addpath(toolboxpath);" << endl << endl;
|
makeModuleMfile.oss << "addpath(toolboxpath);" << endl << endl;
|
||||||
|
|
||||||
makeModuleMakefile.oss << "\nMEX = mex\n";
|
makeModuleMakefile.oss << "\nMEX = " << mexCommand << "\n";
|
||||||
makeModuleMakefile.oss << "MEXENDING = " << mexExt << "\n";
|
makeModuleMakefile.oss << "MEXENDING = " << mexExt << "\n";
|
||||||
makeModuleMakefile.oss << "mex_flags = " << mexFlags << "\n\n";
|
makeModuleMakefile.oss << "mex_flags = " << mexFlags << "\n\n";
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -40,7 +40,9 @@ struct Module {
|
||||||
bool enable_verbose=true);
|
bool enable_verbose=true);
|
||||||
|
|
||||||
/// MATLAB code generation:
|
/// 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& mexExt,
|
||||||
const std::string& mexFlags) const;
|
const std::string& mexFlags) const;
|
||||||
};
|
};
|
||||||
|
|
|
||||||
15
wrap/README
15
wrap/README
|
|
@ -1,14 +1,17 @@
|
||||||
Frank Dellaert
|
Frank Dellaert
|
||||||
October 2011
|
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
|
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
|
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:
|
Some good things to know:
|
||||||
|
|
||||||
OBJECT CREATION
|
OBJECT CREATION
|
||||||
|
|
@ -24,7 +27,5 @@ METHOD (AND CONSTRUCTOR) ARGUMENTS
|
||||||
generation of the correct conversion routines unwrap<Vector> and unwrap<Matrix>
|
generation of the correct conversion routines unwrap<Vector> and unwrap<Matrix>
|
||||||
- passing classes as arguments works, provided they are passed by reference.
|
- passing classes as arguments works, provided they are passed by reference.
|
||||||
This triggers a call to unwrap_shared_ptr
|
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
|
if nargin ~= 13 && obj.self == 0, error('ClassD constructor failed'); end
|
||||||
end
|
end
|
||||||
function delete(obj)
|
function delete(obj)
|
||||||
delete_ClassD(obj)
|
delete_ClassD(obj);
|
||||||
end
|
end
|
||||||
function display(obj), obj.print(''); end
|
function display(obj), obj.print(''); end
|
||||||
function disp(obj), obj.display; end
|
function disp(obj), obj.display; end
|
||||||
|
|
|
||||||
|
|
@ -63,7 +63,7 @@ TEST( wrap, check_exception ) {
|
||||||
|
|
||||||
string path = topdir + "/wrap/tests";
|
string path = topdir + "/wrap/tests";
|
||||||
Module module(path.c_str(), "testDependencies",enable_verbose);
|
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
|
// emit MATLAB code
|
||||||
string exp_path = path + "/tests/expected_namespaces/";
|
string exp_path = path + "/tests/expected_namespaces/";
|
||||||
string act_path = "actual_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_.cpp" , act_path + "new_ClassD_.cpp" ));
|
||||||
EXPECT(files_equal(exp_path + "new_ClassD_.m" , act_path + "new_ClassD_.m" ));
|
EXPECT(files_equal(exp_path + "new_ClassD_.m" , act_path + "new_ClassD_.m" ));
|
||||||
|
|
@ -267,7 +267,7 @@ TEST( wrap, matlab_code ) {
|
||||||
|
|
||||||
// emit MATLAB code
|
// emit MATLAB code
|
||||||
// make_geometry will not compile, use make testwrap to generate real make
|
// 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/Point2.m" , "actual/@Point2/Point2.m" ));
|
||||||
EXPECT(files_equal(path + "/tests/expected/@Point2/x.cpp" , "actual/@Point2/x.cpp" ));
|
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
|
* 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 mexExt is the extension for mex binaries for this os/cpu
|
||||||
* @param interfacePath path to where interface file lives, e.g., borg/gtsam
|
* @param interfacePath path to where interface file lives, e.g., borg/gtsam
|
||||||
* @param moduleName name of the module to be generated e.g. 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 nameSpace e.g. gtsam
|
||||||
* @param mexFlags extra arguments for mex script, i.e., include flags etc...
|
* @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& interfacePath,
|
||||||
const string& moduleName,
|
const string& moduleName,
|
||||||
const string& toolboxPath,
|
const string& toolboxPath,
|
||||||
|
|
@ -42,13 +45,14 @@ void generate_matlab_toolbox(const string& mexExt,
|
||||||
wrap::Module module(interfacePath, moduleName, true);
|
wrap::Module module(interfacePath, moduleName, true);
|
||||||
|
|
||||||
// Then emit MATLAB code
|
// Then emit MATLAB code
|
||||||
module.matlab_code(toolboxPath,mexExt,mexFlags);
|
module.matlab_code(mexCommand,toolboxPath,mexExt,mexFlags);
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Displays usage information */
|
/** Displays usage information */
|
||||||
void usage() {
|
void usage() {
|
||||||
cerr << "wrap parses an interface file and produces a MATLAB toolbox" << endl;
|
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 << " mexExtension : OS/CPU-dependent extension for MEX binaries" << endl;
|
||||||
cerr << " interfacePath : *absolute* path to directory of module interface file" << 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;
|
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
|
* Typically called from "make all" using appropriate arguments
|
||||||
*/
|
*/
|
||||||
int main(int argc, const char* argv[]) {
|
int main(int argc, const char* argv[]) {
|
||||||
if (argc<6 || argc>7) {
|
if (argc<7 || argc>8) {
|
||||||
cerr << "Invalid arguments:\n";
|
cerr << "Invalid arguments:\n";
|
||||||
for (int i=0; i<argc; ++i)
|
for (int i=0; i<argc; ++i)
|
||||||
cerr << argv[i] << endl;
|
cerr << argv[i] << endl;
|
||||||
|
|
@ -69,6 +73,6 @@ int main(int argc, const char* argv[]) {
|
||||||
usage();
|
usage();
|
||||||
}
|
}
|
||||||
else
|
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;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue