12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged develop into feature/LPSolver

release/4.3a0
Ivan Jimenez 2016-02-18 18:49:51 -05:00
parent 3def6cab74 61113b1960
commit 5fab190194
212 changed files with 7942 additions and 3397 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
CMakeLists.txt
View File

@ -64,8 +64,9 @@ option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TB
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON)
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_USE_VECTOR3_POINTS "Symply typdef Point3 to eigen::Vector3d" OFF)
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
@ -84,6 +85,14 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.")
endif()
if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
endif()
# Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
@ -220,8 +229,9 @@ if(GTSAM_USE_SYSTEM_EIGEN)
set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
# check if MKL is also enabled - can have one or the other, but not both!
if(EIGEN_USE_MKL_ALL)
message(FATAL_ERROR "MKL cannot be used together with system-installed Eigen, as MKL support relies on patches which are not yet in the system-installed Eigen. Disable either GTSAM_USE_SYSTEM_EIGEN or GTSAM_WITH_EIGEN_MKL")
# Note: Eigen >= v3.2.5 includes our patches
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
endif()
else()
# Use bundled Eigen include path.
@ -318,10 +328,6 @@ if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
endif()
if(GTSAM_ALLOW_DEPRECATED_SINCE_V4)
add_definitions(-DGTSAM_ALLOW_DEPRECATED_SINCE_V4)
endif()
###############################################################################
# Add components
@ -477,6 +483,7 @@ print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency chec
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")

6
GTSAM-Concepts.md
View File

@ -19,10 +19,10 @@ To optimize over continuous types, we assume they are manifolds. This is central
[Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space.
In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented.
In detail, we ask the following are defined in the traits object:
In detail, we ask the following are defined in the traits object (although, not all are needed for optimization):
* values:
* `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimenionality is only defined at runtime, by specifying the value -1.
* `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
* types:
* `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
* `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.
@ -33,7 +33,7 @@ In detail, we ask the following are defined in the traits object:
* `gtsam::traits::lie_group_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
* `gtsam::traits::vector_space_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
* valid expressions:
* `size_t dim = traits<T>::getDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time.
* `size_t dim = traits<T>::GetDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time.
* `v = traits<T>::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p*, where *p* and *q* are elements of the manifold and the result, *v* is an element of the vector space.
* `p = traits<T>::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v*, where *p* is an element of the manifold and the result, *v* is an element of the vector space.
* invariants

7
README.md
View File

@ -38,6 +38,13 @@ Optional prerequisites - used automatically if findable by CMake:
- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
GTSAM 4 Compatibility
---------------------
GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
Additional Information
----------------------

1399
doc/ImuFactor.lyx Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
doc/ImuFactor.pdf Normal file
View File

Binary file not shown.

592
doc/LieGroups.lyx
View File

@ -1,5 +1,5 @@
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
#LyX 2.1 created this file. For more info see http://www.lyx.org/
\lyxformat 474
\begin_document
\begin_header
\textclass article
@ -15,13 +15,13 @@ theorems-std
\font_roman times
\font_sans default
\font_typewriter default
\font_math auto
\font_default_family rmdefault
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\default_output_format default
\output_sync 0
@ -32,15 +32,24 @@ theorems-std
\use_hyperref false
\papersize default
\use_geometry true
\use_amsmath 1
\use_esint 0
\use_mhchem 1
\use_mathdots 1
\use_package amsmath 1
\use_package amssymb 1
\use_package cancel 1
\use_package esint 0
\use_package mathdots 1
\use_package mathtools 1
\use_package mhchem 1
\use_package stackrel 1
\use_package stmaryrd 1
\use_package undertilde 1
\cite_engine basic
\cite_engine_type default
\biblio_style plain
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\justification true
\use_refstyle 0
\index Index
\shortcut idx
@ -76,335 +85,10 @@ Frank Dellaert
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_inset CommandInset include
LatexCommand include
filename "macros.lyx"
\begin_layout Plain Layout
Derivatives
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}}
{\frac{\partial#1}{\partial#2}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
{#1\biggr\rvert_{#2}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
{\at{\deriv{#1}{#2}}{#3}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
Lie Groups
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\xhat}{\hat{x}}
{\hat{x}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\yhat}{\hat{y}}
{\hat{y}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Ad}[1]{Ad_{#1}}
{Ad_{#1}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}}
{\mathbf{\mathop{Ad}}{}_{#1}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\define}{\stackrel{\Delta}{=}}
{\stackrel{\Delta}{=}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\gg}{\mathfrak{g}}
{\mathfrak{g}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Rn}{\mathbb{R}^{n}}
{\mathbb{R}^{n}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SO(2), 1
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
{\mathfrak{\mathbb{R}^{2}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\SOtwo}{SO(2)}
{SO(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\sotwo}{\mathfrak{so(2)}}
{\mathfrak{so(2)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\that}{\hat{\theta}}
{\hat{\theta}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\skew}[1]{[#1]_{+}}
{[#1]_{+}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SE(2), 3
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\SEtwo}{SE(2)}
{SE(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\setwo}{\mathfrak{se(2)}}
{\mathfrak{se(2)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SO(3), 3
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rthree}{\mathfrak{\mathbb{R}^{3}}}
{\mathfrak{\mathbb{R}^{3}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\SOthree}{SO(3)}
{SO(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\sothree}{\mathfrak{so(3)}}
{\mathfrak{so(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\what}{\hat{\omega}}
{\hat{\omega}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SE(3),6
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rsix}{\mathfrak{\mathbb{R}^{6}}}
{\mathfrak{\mathbb{R}^{6}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\SEthree}{SE(3)}
{SE(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\sethree}{\mathfrak{se(3)}}
{\mathfrak{se(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\xihat}{\hat{\xi}}
{\hat{\xi}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
Aff(2),6
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Afftwo}{Aff(2)}
{Aff(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\afftwo}{\mathfrak{aff(2)}}
{\mathfrak{aff(2)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\aa}{a}
{a}
\end_inset
\begin_inset FormulaMacro
\newcommand{\ahat}{\hat{a}}
{\hat{a}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status collapsed
\begin_layout Plain Layout
SL(3),8
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\SLthree}{SL(3)}
{SL(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\slthree}{\mathfrak{sl(3)}}
{\mathfrak{sl(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\hh}{h}
{h}
\end_inset
\begin_inset FormulaMacro
\newcommand{\hhat}{\hat{h}}
{\hat{h}}
\end_inset
@ -421,7 +105,7 @@ We will start with a small example of a robot moving in a plane, parameterized
2D pose
\emph default
\begin_inset Formula $(x,\, y,\,\theta)$
\begin_inset Formula $(x,\,y,\,\theta)$
\end_inset
.
@ -460,7 +144,7 @@ A similar story holds for translation in the
direction, and in fact for translations in general:
\begin_inset Formula
\[
(x_{t},\, y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\, y_{0}+v_{y}t,\,\theta_{0})
(x_{t},\,y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\,y_{0}+v_{y}t,\,\theta_{0})
\]
\end_inset
@ -468,7 +152,7 @@ A similar story holds for translation in the
Similarly for rotation we have
\begin_inset Formula
\[
(x_{t},\, y_{t},\,\theta_{t})=(x_{0},\, y_{0},\,\theta_{0}+\omega t)
(x_{t},\,y_{t},\,\theta_{t})=(x_{0},\,y_{0},\,\theta_{0}+\omega t)
\]
\end_inset
@ -500,7 +184,7 @@ status collapsed
\end_inset
\begin_inset Caption
\begin_inset Caption Standard
\begin_layout Plain Layout
Robot moving along a circular trajectory.
@ -521,20 +205,20 @@ However, if we combine translation and rotation, the story breaks down!
We cannot write
\begin_inset Formula
\[
(x_{t},\, y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\, y_{0}+v_{y}t,\,\theta_{0}+\omega t)
(x_{t},\,y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\,y_{0}+v_{y}t,\,\theta_{0}+\omega t)
\]
\end_inset
The reason is that, if we move the robot a tiny bit according to the velocity
vector
\begin_inset Formula $(v_{x},\, v_{y},\,\omega)$
\begin_inset Formula $(v_{x},\,v_{y},\,\omega)$
\end_inset
, we have (to first order)
\begin_inset Formula
\[
(x_{\delta},\, y_{\delta},\,\theta_{\delta})=(x_{0}+v_{x}\delta,\, y_{0}+v_{y}\delta,\,\theta_{0}+\omega\delta)
(x_{\delta},\,y_{\delta},\,\theta_{\delta})=(x_{0}+v_{x}\delta,\,y_{0}+v_{y}\delta,\,\theta_{0}+\omega\delta)
\]
\end_inset
@ -580,7 +264,7 @@ status open
\end_inset
\begin_inset Caption
\begin_inset Caption Standard
\begin_layout Plain Layout
\begin_inset CommandInset label
@ -615,7 +299,7 @@ To make progress, we have to be more precise about how the robot behaves.
as
\begin_inset Formula
\[
T_{1}T_{2}=(x_{1},\, y_{1},\,\theta_{1})(x_{2},\, y_{2},\,\theta_{2})=(x_{1}+\cos\theta_{1}x_{2}-\sin\theta y_{2},\, y_{1}+\sin\theta_{1}x_{2}+\cos\theta_{1}y_{2},\,\theta_{1}+\theta_{2})
T_{1}T_{2}=(x_{1},\,y_{1},\,\theta_{1})(x_{2},\,y_{2},\,\theta_{2})=(x_{1}+\cos\theta_{1}x_{2}-\sin\theta y_{2},\,y_{1}+\sin\theta_{1}x_{2}+\cos\theta_{1}y_{2},\,\theta_{1}+\theta_{2})
\]
\end_inset
@ -1925,13 +1609,13 @@ Hence, an alternative way of writing down elements of
\end_inset
is as the ordered pair
\begin_inset Formula $(R,\, t)$
\begin_inset Formula $(R,\,t)$
\end_inset
, with composition defined a
\begin_inset Formula
\[
(R_{1},\, t_{1})(R_{2},\, t_{2})=(R_{1}R_{2},\, R{}_{1}t_{2}+t_{1})
(R_{1},\,t_{1})(R_{2},\,t_{2})=(R_{1}R_{2},\,R{}_{1}t_{2}+t_{1})
\]
\end_inset
@ -2894,13 +2578,13 @@ where
\end_inset
is as the ordered pair
\begin_inset Formula $(R,\, t)$
\begin_inset Formula $(R,\,t)$
\end_inset
, with composition defined as
\begin_inset Formula
\[
(R_{1},\, t_{1})(R_{2},\, t_{2})=(R_{1}R_{2},\, R{}_{1}t_{2}+t_{1})
(R_{1},\,t_{1})(R_{2},\,t_{2})=(R_{1}R_{2},\,R{}_{1}t_{2}+t_{1})
\]
\end_inset
@ -3269,6 +2953,218 @@ p\\
\end_inset
\end_layout
\begin_layout Section
3D Similarity Transformations
\end_layout
\begin_layout Standard
The group of 3D similarity transformations
\begin_inset Formula $Sim(3)$
\end_inset
is the set of
\begin_inset Formula $4\times4$
\end_inset
invertible matrices of the form
\begin_inset Formula
\[
T\define\left[\begin{array}{cc}
R & t\\
0 & s^{-1}
\end{array}\right]
\]
\end_inset
where
\begin_inset Formula $s$
\end_inset
is a scalar.
There are several different conventions in use for the Lie algebra generators,
but we use
\begin_inset Formula
\[
G^{1}=\left(\begin{array}{cccc}
0 & 0 & 0 & 0\\
0 & 0 & -1 & 0\\
0 & 1 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{}G^{2}=\left(\begin{array}{cccc}
0 & 0 & 1 & 0\\
0 & 0 & 0 & 0\\
-1 & 0 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{ }G^{3}=\left(\begin{array}{cccc}
0 & -1 & 0 & 0\\
1 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)
\]
\end_inset
\begin_inset Formula
\[
G^{4}=\left(\begin{array}{cccc}
0 & 0 & 0 & 1\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{}G^{5}=\left(\begin{array}{cccc}
0 & 0 & 0 & 0\\
0 & 0 & 0 & 1\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{ }G^{6}=\left(\begin{array}{cccc}
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 1\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{ }G^{7}=\left(\begin{array}{cccc}
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & -1
\end{array}\right)
\]
\end_inset
\end_layout
\begin_layout Subsection
Actions
\end_layout
\begin_layout Standard
The action of
\begin_inset Formula $\SEthree$
\end_inset
on 3D points is done by embedding the points in
\begin_inset Formula $\mathbb{R}^{4}$
\end_inset
by using homogeneous coordinates
\begin_inset Formula
\[
\hat{q}=\left[\begin{array}{c}
q\\
s^{-1}
\end{array}\right]=\left[\begin{array}{c}
Rp+t\\
s^{-1}
\end{array}\right]=\left[\begin{array}{cc}
R & t\\
0 & s^{-1}
\end{array}\right]\left[\begin{array}{c}
p\\
1
\end{array}\right]=T\hat{p}
\]
\end_inset
The derivative
\begin_inset Formula $D_{1}f(\xi)$
\end_inset
in an incremental change
\begin_inset Formula $\xi$
\end_inset
to
\begin_inset Formula $T$
\end_inset
is given by
\begin_inset Formula $TH(p)$
\end_inset
where
\begin_inset Formula
\[
H(p)=G_{jk}^{i}p^{j}=\left(\begin{array}{ccccccc}
0 & z & -y & 1 & 0 & 0 & 0\\
-z & 0 & x & 0 & 1 & 0 & 0\\
y & -x & 0 & 0 & 0 & 1 & 0\\
0 & 0 & 0 & 0 & 0 & 0 & -1
\end{array}\right)
\]
\end_inset
In other words
\begin_inset Formula
\[
D_{1}f(\xi)=\left[\begin{array}{cc}
R & t\\
0 & s^{-1}
\end{array}\right]\left[\begin{array}{ccc}
-\left[p\right]_{x} & I_{3} & 0\\
0 & 0 & -1
\end{array}\right]=\left[\begin{array}{ccc}
-R\left[p\right]_{x} & R & -t\\
0 & 0 & -s^{-1}
\end{array}\right]
\]
\end_inset
This is the derivative for the action on homogeneous coordinates.
Switching back to non-homogeneous coordinates is done by
\begin_inset Formula
\[
\left[\begin{array}{c}
q\\
a
\end{array}\right]\rightarrow q/a
\]
\end_inset
with derivative
\begin_inset Formula
\[
\left[\begin{array}{cc}
a^{-1}I_{3} & -qa^{-2}\end{array}\right]
\]
\end_inset
For
\begin_inset Formula $a=s^{-1}$
\end_inset
we obtain
\begin_inset Formula
\[
D_{1}f(\xi)=\left[\begin{array}{cc}
sI_{3} & -qs^{2}\end{array}\right]\left[\begin{array}{ccc}
-R\left[p\right]_{x} & R & -t\\
0 & 0 & -s^{-1}
\end{array}\right]=\left[\begin{array}{ccc}
-sR\left[p\right]_{x} & sR & -st+qs\end{array}\right]=\left[\begin{array}{ccc}
-sR\left[p\right]_{x} & sR & sRp\end{array}\right]
\]
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Newpage pagebreak
\end_inset
\end_layout
\begin_layout Section

BIN
doc/LieGroups.pdf
View File

Binary file not shown.

137
doc/macros.lyx
View File

@ -1,42 +1,60 @@
#LyX 1.6.5 created this file. For more info see http://www.lyx.org/
\lyxformat 345
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
\begin_document
\begin_header
\textclass article
\use_default_options true
\maintain_unincluded_children false
\language english
\language_package default
\inputencoding auto
\fontencoding global
\font_roman default
\font_sans default
\font_typewriter default
\font_default_family default
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\default_output_format default
\output_sync 0
\bibtex_command default
\index_command default
\paperfontsize default
\use_hyperref false
\papersize default
\use_geometry false
\use_amsmath 1
\use_esint 1
\use_mhchem 1
\use_mathdots 0
\cite_engine basic
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\use_refstyle 0
\index Index
\shortcut idx
\color #008000
\end_index
\secnumdepth 3
\tocdepth 3
\paragraph_separation indent
\defskip medskip
\paragraph_indentation default
\quotes_language english
\papercolumns 1
\papersides 1
\paperpagestyle default
\tracking_changes false
\output_changes false
\author ""
\author ""
\html_math_output 0
\html_css_as_file 0
\html_be_strict false
\end_header
\begin_body
@ -62,14 +80,14 @@ Derivatives
\begin_inset FormulaMacro
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
{#1\biggr\rvert_{#2}}
\newcommand{\at}[1]{#1\biggr\vert_{\#2}}
{#1\biggr\vert_{\#2}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
{\at{\deriv{#1}{#2}}{#3}}
{\at{\deriv{#1}{#2}}#3}
\end_inset
@ -107,6 +125,15 @@ Lie Groups
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}}
{\mathbf{\mathop{Ad}}{}_{#1}}
\end_inset
\end_layout
\begin_layout Standard
@ -144,6 +171,12 @@ SO(2)
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rone}{\mathfrak{\mathbb{R}}}
{\mathfrak{\mathbb{R}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
{\mathfrak{\mathbb{R}^{2}}}
@ -202,6 +235,12 @@ SE(2)
\end_inset
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset
\end_layout
\begin_layout Standard
@ -243,7 +282,7 @@ SO(3)
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
\renewcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset
@ -288,6 +327,86 @@ SE(3)
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
Aff(2),6
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Afftwo}{Aff(2)}
{Aff(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\afftwo}{\mathfrak{aff(2)}}
{\mathfrak{aff(2)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\aa}{a}
{a}
\end_inset
\begin_inset FormulaMacro
\newcommand{\ahat}{\hat{a}}
{\hat{a}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status collapsed
\begin_layout Plain Layout
SL(3),8
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\SLthree}{SL(3)}
{SL(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\slthree}{\mathfrak{sl(3)}}
{\mathfrak{sl(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\hh}{h}
{h}
\end_inset
\begin_inset FormulaMacro
\newcommand{\hhat}{\hat{h}}
{\hat{h}}
\end_inset
\end_layout
\end_body

437
doc/math.lyx
View File

@ -1,5 +1,5 @@
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
#LyX 2.1 created this file. For more info see http://www.lyx.org/
\lyxformat 474
\begin_document
\begin_header
\textclass article
@ -17,13 +17,13 @@ theorems-ams-bytype
\font_roman times
\font_sans default
\font_typewriter default
\font_math auto
\font_default_family rmdefault
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\default_output_format default
\output_sync 0
@ -34,15 +34,24 @@ theorems-ams-bytype
\use_hyperref false
\papersize default
\use_geometry true
\use_amsmath 1
\use_esint 0
\use_mhchem 1
\use_mathdots 1
\use_package amsmath 1
\use_package amssymb 1
\use_package cancel 1
\use_package esint 0
\use_package mathdots 1
\use_package mathtools 1
\use_package mhchem 1
\use_package stackrel 0
\use_package stmaryrd 1
\use_package undertilde 1
\cite_engine basic
\cite_engine_type default
\biblio_style plain
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\justification true
\use_refstyle 0
\index Index
\shortcut idx
@ -1237,21 +1246,28 @@ reference "eq:ApproximateObjective"
\end_inset
.
In particular, the notion of an exponential map allows us to define an
incremental transformation as tracing out a geodesic curve on the group
manifold along a certain
In particular, the notion of an exponential map allows us to define a mapping
from
\series bold
tangent vector
local coordinates
\series default
\begin_inset Formula $\xi$
\end_inset
back to a neighborhood in
\begin_inset Formula $G$
\end_inset
around
\begin_inset Formula $a$
\end_inset
,
\begin_inset Formula
\[
a\oplus\xi\define a\exp\left(\hat{\xi}\right)
\]
\begin{equation}
a\oplus\xi\define a\exp\left(\hat{\xi}\right)\label{eq:expmap}
\end{equation}
\end_inset
@ -1263,11 +1279,12 @@ with
\begin_inset Formula $n$
\end_inset
-dimensional Lie group,
-dimensional Lie group.
Above,
\begin_inset Formula $\hat{\xi}\in\mathfrak{g}$
\end_inset
the Lie algebra element corresponding to the vector
is the Lie algebra element corresponding to the vector
\begin_inset Formula $\xi$
\end_inset
@ -1305,7 +1322,7 @@ For the Lie group
\end_inset
is denoted as
\begin_inset Formula $\omega$
\begin_inset Formula $\omega t$
\end_inset
and represents an angular displacement.
@ -1314,17 +1331,17 @@ For the Lie group
\end_inset
is a skew symmetric matrix denoted as
\begin_inset Formula $\Skew{\omega}\in\sothree$
\begin_inset Formula $\Skew{\omega t}\in\sothree$
\end_inset
, and is given by
\begin_inset Formula
\[
\Skew{\omega}=\left[\begin{array}{ccc}
\Skew{\omega t}=\left[\begin{array}{ccc}
0 & -\omega_{z} & \omega_{y}\\
\omega_{z} & 0 & -\omega_{x}\\
-\omega_{y} & \omega_{x} & 0
\end{array}\right]
\end{array}\right]t
\]
\end_inset
@ -1334,12 +1351,136 @@ Finally, the increment
\end_inset
corresponds to an incremental rotation
\begin_inset Formula $R\oplus\omega=Re^{\Skew{\omega}}$
\begin_inset Formula $R\oplus\omega t=Re^{\Skew{\omega t}}$
\end_inset
.
\end_layout
\begin_layout Subsection
Local Coordinates vs.
Tangent Vectors
\end_layout
\begin_layout Standard
In differential geometry,
\series bold
tangent vectors
\series default
\begin_inset Formula $v\in T_{a}G$
\end_inset
at
\begin_inset Formula $a$
\end_inset
are elements of the Lie algebra
\begin_inset Formula $\mathfrak{g}$
\end_inset
, and are defined as
\begin_inset Formula
\[
v\define\Jac{\gamma(t)}t{t=0}
\]
\end_inset
where
\begin_inset Formula $\gamma$
\end_inset
is some curve that passes through
\begin_inset Formula $a$
\end_inset
at
\begin_inset Formula $t=0$
\end_inset
, i.e.
\begin_inset Formula $\gamma(0)=a$
\end_inset
.
In particular, for any fixed local coordinate
\begin_inset Formula $\xi$
\end_inset
the map
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:expmap"
\end_inset
can be used to define a
\series bold
geodesic curve
\series default
on the group manifold defined by
\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
\end_inset
, and the corresponding tangent vector is given by
\begin_inset Formula
\begin{equation}
\Jac{ae^{\widehat{t\xi}}}t{t=0}=a\xihat\label{eq:tangent-vector}
\end{equation}
\end_inset
This defines the mapping between local coordinates
\begin_inset Formula $\xi\in\Rn$
\end_inset
and actual tangent vectors
\begin_inset Formula $a\xihat\in g$
\end_inset
: the vector
\begin_inset Formula $\xi$
\end_inset
defines a direction of travel on the manifold, but does so in the local
coordinate frame
\begin_inset Formula $a$
\end_inset
.
\end_layout
\begin_layout Example
Assume a rigid body's attitude is described by
\begin_inset Formula $R_{b}^{n}(t)$
\end_inset
, where the indices denote the navigation frame
\begin_inset Formula $N$
\end_inset
and body frame
\begin_inset Formula $B$
\end_inset
, respectively.
An extrinsically calibrated gyroscope measures the angular velocity
\begin_inset Formula $\omega^{b}$
\end_inset
, in the body frame, and the corresponding tangent vector is
\begin_inset Formula
\[
\dot{R}_{b}^{n}(t)=R_{b}^{n}(t)\widehat{\omega^{b}}
\]
\end_inset
\end_layout
\begin_layout Subsection
Derivatives
\end_layout
@ -1352,7 +1493,7 @@ reference "def:differentiable"
\end_inset
to map exponential coordinates
to map local coordinates
\begin_inset Formula $\xi$
\end_inset
@ -1368,7 +1509,7 @@ reference "def:differentiable"
\begin_inset Formula $Df_{a}$
\end_inset
locally approximates a function
approximates the function
\begin_inset Formula $f$
\end_inset
@ -1378,6 +1519,10 @@ reference "def:differentiable"
to
\begin_inset Formula $\Reals m$
\end_inset
in a neighborhood around
\begin_inset Formula $a$
\end_inset
:
@ -1455,41 +1600,6 @@ derivative
.
\end_layout
\begin_layout Standard
Note that the vectors
\begin_inset Formula $\xi$
\end_inset
can be viewed as lying in the tangent space to
\begin_inset Formula $G$
\end_inset
at
\begin_inset Formula $a$
\end_inset
, but defining this rigorously would take us on a longer tour of differential
geometry.
Informally,
\begin_inset Formula $\xi$
\end_inset
is simply the direction, in a local coordinate frame, that is locally tangent
at
\begin_inset Formula $a$
\end_inset
to a geodesic curve
\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
\end_inset
traced out by the exponential map, with
\begin_inset Formula $\gamma(0)=a$
\end_inset
.
\end_layout
\begin_layout Subsection
Derivative of an Action
\begin_inset CommandInset label
@ -1502,11 +1612,7 @@ name "sec:Derivatives-of-Actions"
\end_layout
\begin_layout Standard
The (usual) action of an
\begin_inset Formula $n$
\end_inset
-dimensional matrix group
The (usual) action of a matrix group
\begin_inset Formula $G$
\end_inset
@ -1531,7 +1637,7 @@ Since this is a function defined on the product
\end_inset
the derivative is a linear transformation
\begin_inset Formula $Df:\Multi{2n}n$
\begin_inset Formula $Df:\Multi{m+n}n$
\end_inset
with
@ -1542,7 +1648,15 @@ Df_{(T,p)}\left(\xi,\delta p\right)=D_{1}f_{(T,p)}\left(\xi\right)+D_{2}f_{(T,p)
\end_inset
where
\begin_inset Formula $m$
\end_inset
is the dimensionality of the manifold
\begin_inset Formula $G$
\end_inset
.
\end_layout
\begin_layout Theorem
@ -1571,7 +1685,7 @@ H(p) & I_{n}\end{array}\right]
\end_inset
with
\begin_inset Formula $H:\Reals n\rightarrow\Reals{n\times n}$
\begin_inset Formula $H:\Reals m\rightarrow\Reals{n\times m}$
\end_inset
a linear mapping that depends on
@ -1698,7 +1812,7 @@ with
\end_inset
an
\begin_inset Formula $n\times n$
\begin_inset Formula $n\times m$
\end_inset
matrix that depends on
@ -3000,7 +3114,7 @@ f(ge^{\xhat})=f(ge^{\xhat}g^{-1}g)=f(e^{\Ad g\xhat}g)
\end_layout
\begin_layout Subsection
Derivative of the Exponential and Logarithm Map
Derivative of the Exponential Map
\end_layout
\begin_layout Theorem
@ -3064,17 +3178,196 @@ For
\begin_inset Formula $\xi\neq0$
\end_inset
, things are not simple, see .
, things are not simple.
As with pushforwards above, we will be looking for an
\begin_inset Formula $n\times n$
\end_inset
\begin_inset Flex URL
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none
Jacobian
\begin_inset Formula $f'(\xi)$
\end_inset
such that
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\uuline default
\uwave default
\noun default
\color inherit
\begin_inset Formula
\begin{equation}
f\left(\xi+\delta\right)\approx f\left(\xi\right)\exp\left(\widehat{f'(\xi)\delta}\right)\label{eq:push_exp}
\end{equation}
\end_inset
Differential geometry tells us that for any Lie algebra element
\begin_inset Formula $\xihat\in\mathfrak{g}$
\end_inset
there exists a
\emph on
linear
\emph default
map
\begin_inset Formula $d\exp_{\xihat}:T_{\xihat}\mathfrak{g}\rightarrow T_{\exp(\xihat)}G$
\end_inset
, which is given by
\begin_inset Foot
status collapsed
\begin_layout Plain Layout
See
\begin_inset Flex URL
status open
http://deltaepsilons.wordpress.com/2009/11/06/helgasons-formula-for-the-differenti
al-of-the-exponential/
\begin_layout Plain Layout
http://deltaepsilons.wordpress.com/2009/11/06/
\end_layout
\end_inset
or
\begin_inset Flex URL
status open
\begin_layout Plain Layout
https://en.wikipedia.org/wiki/Derivative_of_the_exponential_map
\end_layout
\end_inset
.
\end_layout
\end_inset
\begin_inset Formula
\begin{equation}
d\exp_{\xihat}\hat{x}=\exp(\xihat)\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}\label{eq:dexp}
\end{equation}
\end_inset
with
\begin_inset Formula $\hat{x}\in T_{\xihat}\mathfrak{g}$
\end_inset
and
\begin_inset Formula $ad_{\xihat}$
\end_inset
itself a linear map taking
\begin_inset Formula $\hat{x}$
\end_inset
to
\begin_inset Formula $[\xihat,\hat{x}]$
\end_inset
, the Lie bracket.
The actual formula above is not really as important as the fact that the
linear map exists, although it is expressed directly in terms of tangent
vectors to
\begin_inset Formula $\mathfrak{g}$
\end_inset
and
\begin_inset Formula $G$
\end_inset
.
Equation
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:dexp"
\end_inset
is a tangent vector, and comparing with
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:tangent-vector"
\end_inset
we see that it maps to local coordinates
\begin_inset Formula $y$
\end_inset
as follows:
\begin_inset Formula
\[
\yhat=\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}
\]
\end_inset
which can be used to construct the Jacobian
\begin_inset Formula $f'(\xi)$
\end_inset
.
\end_layout
\begin_layout Example
For
\begin_inset Formula $\SOthree$
\end_inset
, the operator
\begin_inset Formula $ad_{\xihat}$
\end_inset
is simply a matrix multiplication when representing
\begin_inset Formula $\sothree$
\end_inset
using 3-vectors, i.e.,
\begin_inset Formula $ad_{\xihat}x=\xihat x$
\end_inset
, and the
\begin_inset Formula $3\times3$
\end_inset
Jacobian corresponding to
\begin_inset Formula $d\exp$
\end_inset
is
\begin_inset Formula
\[
f'(\xi)=\frac{I_{3\times3}-\exp(-\xihat)}{\xihat}=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\xihat^{k}
\]
\end_inset
which, similar to the exponential map, has a simple closed form expression
for
\begin_inset Formula $\SOthree$
\end_inset
.
@ -6797,7 +7090,7 @@ Then
\begin_layout Standard
\begin_inset CommandInset bibtex
LatexCommand bibtex
bibfiles "/Users/dellaert/papers/refs"
bibfiles "refs"
options "plain"
\end_inset

BIN
doc/math.pdf
View File

Binary file not shown.

26
doc/refs.bib Normal file
View File

@ -0,0 +1,26 @@
@article{Iserles00an,
title = {Lie-group methods},
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and
N{\o}rsett, Syvert P and Zanna, Antonella},
journal = {Acta Numerica 2000},
volume = {9},
pages = {215--365},
year = {2000},
publisher = {Cambridge Univ Press}
}
@book{Murray94book,
title = {A mathematical introduction to robotic manipulation},
author = {Murray, Richard M and Li, Zexiang and Sastry, S
Shankar and Sastry, S Shankara},
year = {1994},
publisher = {CRC press}
}
@book{Spivak65book,
title = {Calculus on manifolds},
author = {Spivak, Michael},
volume = {1},
year = {1965},
publisher = {WA Benjamin New York}
}

2
examples/RangeISAMExample_plaza2.cpp
View File

@ -83,7 +83,7 @@ vector<RangeTriple> readTriples() {
while (is) {
double t, sender, range;
size_t receiver;
size_t receiver;
is >> t >> sender >> receiver >> range;
triples.push_back(RangeTriple(t, receiver, range));
}

2
examples/SFMExample.cpp
View File

@ -99,7 +99,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
initialEstimate.print("Initial Estimates:\n");
/* Optimize the graph and print results */

2
examples/SFMExampleExpressions.cpp
View File

@ -88,7 +88,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initial.insert(Symbol('x', i), poses[i].compose(delta));
for (size_t j = 0; j < points.size(); ++j)
initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
cout << "initial error = " << graph.error(initial) << endl;
/* Optimize the graph and print results */

2
examples/SelfCalibrationExample.cpp
View File

@ -84,7 +84,7 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();

2
examples/VisualISAM2Example.cpp
View File

@ -113,7 +113,7 @@ int main(int argc, char* argv[]) {
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15));
} else {
// Update iSAM with the new factors

2
examples/VisualISAMExample.cpp
View File

@ -120,7 +120,7 @@ int main(int argc, char* argv[]) {
Point3 noise(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j) {
// Intentionally initialize the variables off from the ground truth
Point3 initial_lj = points[j].compose(noise);
Point3 initial_lj = points[j] + noise;
initialEstimate.insert(Symbol('l', j), initial_lj);
}

189
gtsam.h
View File

@ -127,13 +127,13 @@ namespace std {
void pop_back();*/
};
//typedef std::vector
#include<list>
template<T>
class list
{
};
}
@ -360,17 +360,6 @@ class Point3 {
// Group
static gtsam::Point3 identity();
gtsam::Point3 inverse() const;
gtsam::Point3 compose(const gtsam::Point3& p2) const;
gtsam::Point3 between(const gtsam::Point3& p2) const;
// Manifold
gtsam::Point3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point3& p) const;
// Lie Group
static gtsam::Point3 Expmap(Vector v);
static Vector Logmap(const gtsam::Point3& p);
// Standard Interface
Vector vector() const;
@ -1069,7 +1058,7 @@ class SymbolicBayesTree {
void clear();
void deleteCachedShortcuts();
size_t numCachedSeparatorMarginals() const;
gtsam::SymbolicConditional* marginalFactor(size_t key) const;
gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
@ -1079,19 +1068,19 @@ class SymbolicBayesTree {
// BayesTreeClique();
// BayesTreeClique(CONDITIONAL* conditional);
// // BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
//
//
// bool equals(const This& other, double tol) const;
// void print(string s) const;
// void printTree() const; // Default indent of ""
// void printTree(string indent) const;
// size_t numCachedSeparatorMarginals() const;
//
//
// CONDITIONAL* conditional() const;
// bool isRoot() const;
// size_t treeSize() const;
// // const std::list<derived_ptr>& children() const { return children_; }
// // derived_ptr parent() const { return parent_.lock(); }
//
//
// // FIXME: need wrapped versions graphs, BayesNet
// // BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
// // FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
@ -1345,7 +1334,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
gtsam::JacobianFactor whiten() const;
pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
void setModel(bool anyConstrained, const Vector& sigmas);
@ -1422,7 +1411,7 @@ class GaussianFactorGraph {
gtsam::GaussianFactorGraph clone() const;
gtsam::GaussianFactorGraph negate() const;
// Optimizing and linear algebra
// Optimizing and linear algebra
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const;
gtsam::VectorValues optimizeGradientSearch() const;
@ -1511,7 +1500,7 @@ virtual class GaussianBayesNet {
//Constructors
GaussianBayesNet();
GaussianBayesNet(const gtsam::GaussianConditional* conditional);
// Testable
void print(string s) const;
bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
@ -1527,7 +1516,7 @@ virtual class GaussianBayesNet {
gtsam::GaussianConditional* back() const;
void push_back(gtsam::GaussianConditional* conditional);
void push_back(const gtsam::GaussianBayesNet& bayesNet);
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
gtsam::VectorValues optimizeGradientSearch() const;
@ -1551,7 +1540,7 @@ virtual class GaussianBayesTree {
bool empty() const;
size_t numCachedSeparatorMarginals() const;
void saveGraph(string s) const;
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
@ -1833,7 +1822,7 @@ class Values {
/// Fixed size versions, for n in 1..9
Vector atFixed(size_t j, size_t n);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
@ -2004,7 +1993,7 @@ virtual class NonlinearOptimizerParams {
void setVerbosity(string s);
string getLinearSolverType() const;
void setLinearSolverType(string solver);
void setOrdering(const gtsam::Ordering& ordering);
void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
@ -2447,7 +2436,7 @@ namespace imuBias {
#include <gtsam/navigation/ImuBias.h>
class ConstantBias {
// Standard Constructor
// Constructors
ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro);
@ -2479,97 +2468,107 @@ class ConstantBias {
}///\namespace imuBias
#include <gtsam/navigation/NavState.h>
class NavState {
// Constructors
NavState();
NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v);
NavState(const gtsam::Pose3& pose, Vector v);
// Testable
void print(string s) const;
bool equals(const gtsam::NavState& expected, double tol) const;
// Access
gtsam::Rot3 attitude() const;
gtsam::Point3 position() const;
Vector velocity() const;
gtsam::Pose3 pose() const;
};
#include <gtsam/navigation/PreintegrationParams.h>
class PreintegrationParams {
PreintegrationParams(Vector n_gravity);
// TODO(frank): add setters/getters or make this MATLAB wrapper feature
};
#include <gtsam/navigation/PreintegrationBase.h>
virtual class PreintegrationBase {
// Constructors
PreintegrationBase(const gtsam::PreintegrationParams* params);
PreintegrationBase(const gtsam::PreintegrationParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegrationBase& expected, double tol);
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
// Standard Interface
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
#include <gtsam/navigation/ImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class PreintegratedImuMeasurements {
// Standard Constructor
PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
// PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs);
virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
// Constructors
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
};
virtual class ImuFactor : gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
virtual class ImuFactor: gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
size_t bias,
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements);
// Standard Interface
gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j,
const gtsam::imuBias::ConstantBias& bias);
};
#include <gtsam/navigation/CombinedImuFactor.h>
class PreintegratedCombinedMeasurements {
// Standard Constructor
PreintegratedCombinedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
PreintegratedCombinedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
// PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs);
virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol);
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
};
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
virtual class CombinedImuFactor: gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j,
size_t bias_i, size_t bias_j,
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);
// Standard Interface
gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::imuBias::ConstantBias& bias_j);
};
#include <gtsam/navigation/AHRSFactor.h>

4
gtsam/3rdparty/Eigen/.hg_archival.txt vendored
View File

@ -1,4 +1,4 @@
repo: 8a21fd850624c931e448cbcfb38168cb2717c790
node: b30b87236a1b1552af32ac34075ee5696a9b5a33
node: 07105f7124f9aef00a68c85e0fc606e65d3d6c15
branch: 3.2
tag: 3.2.7
tag: 3.2.8

1
gtsam/3rdparty/Eigen/.hgtags vendored
View File

@ -30,3 +30,4 @@ ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4
bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5
c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6
b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7

66
gtsam/3rdparty/Eigen/CMakeLists.txt vendored
View File

@ -1,6 +1,5 @@
project(Eigen)
cmake_minimum_required(VERSION 2.8.2)
cmake_minimum_required(VERSION 2.8.5)
# guard against in-source builds
@ -55,6 +54,7 @@ endif(EIGEN_HG_CHANGESET)
include(CheckCXXCompilerFlag)
include(GNUInstallDirs)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
@ -288,25 +288,26 @@ option(EIGEN_TEST_C++0x "Enables all C++0x features." OFF)
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
# the user modifiable install path for header files
set(EIGEN_INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} CACHE PATH "The directory where we install the header files (optional)")
# set the internal install path for header files which depends on wether the user modifiable
# EIGEN_INCLUDE_INSTALL_DIR has been set by the user or not.
if(EIGEN_INCLUDE_INSTALL_DIR)
set(INCLUDE_INSTALL_DIR
${EIGEN_INCLUDE_INSTALL_DIR}
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
# Backward compatibility support for EIGEN_INCLUDE_INSTALL_DIR
if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR)
set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR}
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed")
else()
set(INCLUDE_INSTALL_DIR
"include/eigen3"
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
"${CMAKE_INSTALL_INCLUDEDIR}/eigen3"
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed"
)
endif()
set(CMAKEPACKAGE_INSTALL_DIR
"${CMAKE_INSTALL_LIBDIR}/cmake/eigen3"
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed"
)
set(PKGCONFIG_INSTALL_DIR
"${CMAKE_INSTALL_DATADIR}/pkgconfig"
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed"
)
# similar to set_target_properties but append the property instead of overwriting it
macro(ei_add_target_property target prop value)
@ -324,21 +325,9 @@ install(FILES
)
if(EIGEN_BUILD_PKGCONFIG)
SET(path_separator ":")
STRING(REPLACE ${path_separator} ";" pkg_config_libdir_search "$ENV{PKG_CONFIG_LIBDIR}")
message(STATUS "searching for 'pkgconfig' directory in PKG_CONFIG_LIBDIR ( $ENV{PKG_CONFIG_LIBDIR} ), ${CMAKE_INSTALL_PREFIX}/share, and ${CMAKE_INSTALL_PREFIX}/lib")
FIND_PATH(pkg_config_libdir pkgconfig ${pkg_config_libdir_search} ${CMAKE_INSTALL_PREFIX}/share ${CMAKE_INSTALL_PREFIX}/lib ${pkg_config_libdir_search})
if(pkg_config_libdir)
SET(pkg_config_install_dir ${pkg_config_libdir})
message(STATUS "found ${pkg_config_libdir}/pkgconfig" )
else(pkg_config_libdir)
SET(pkg_config_install_dir ${CMAKE_INSTALL_PREFIX}/share)
message(STATUS "pkgconfig not found; installing in ${pkg_config_install_dir}" )
endif(pkg_config_libdir)
configure_file(eigen3.pc.in eigen3.pc)
configure_file(eigen3.pc.in eigen3.pc @ONLY)
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc
DESTINATION ${pkg_config_install_dir}/pkgconfig
DESTINATION ${PKGCONFIG_INSTALL_DIR}
)
endif(EIGEN_BUILD_PKGCONFIG)
@ -401,12 +390,15 @@ if(cmake_generator_tolower MATCHES "makefile")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "Command | Description")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "make install | Install to ${CMAKE_INSTALL_PREFIX}. To change that:")
message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourpath")
message(STATUS " | Eigen headers will then be installed to:")
message(STATUS " | ${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}")
message(STATUS " | To install Eigen headers to a separate location, do:")
message(STATUS " | cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath")
message(STATUS "make install | Install Eigen. Headers will be installed to:")
message(STATUS " | <CMAKE_INSTALL_PREFIX>/<INCLUDE_INSTALL_DIR>")
message(STATUS " | Using the following values:")
message(STATUS " | CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
message(STATUS " | INCLUDE_INSTALL_DIR: ${INCLUDE_INSTALL_DIR}")
message(STATUS " | Change the install location of Eigen headers using:")
message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourprefix")
message(STATUS " | Or:")
message(STATUS " | cmake . -DINCLUDE_INSTALL_DIR=yourdir")
message(STATUS "make doc | Generate the API documentation, requires Doxygen & LaTeX")
message(STATUS "make check | Build and run the unit-tests. Read this page:")
message(STATUS " | http://eigen.tuxfamily.org/index.php?title=Tests")

2
gtsam/3rdparty/Eigen/Eigen/CholmodSupport vendored
View File

@ -12,7 +12,7 @@ extern "C" {
/** \ingroup Support_modules
* \defgroup CholmodSupport_Module CholmodSupport module
*
* This module provides an interface to the Cholmod library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* This module provides an interface to the Cholmod library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
* It provides the two following main factorization classes:
* - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization.
* - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial).

2
gtsam/3rdparty/Eigen/Eigen/SPQRSupport vendored
View File

@ -10,7 +10,7 @@
/** \ingroup Support_modules
* \defgroup SPQRSupport_Module SuiteSparseQR module
*
* This module provides an interface to the SPQR library, which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* This module provides an interface to the SPQR library, which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
*
* \code
* #include <Eigen/SPQRSupport>

2
gtsam/3rdparty/Eigen/Eigen/UmfPackSupport vendored
View File

@ -12,7 +12,7 @@ extern "C" {
/** \ingroup Support_modules
* \defgroup UmfPackSupport_Module UmfPackSupport module
*
* This module provides an interface to the UmfPack library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* This module provides an interface to the UmfPack library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
* It provides the following factorization class:
* - class UmfPackLU: a multifrontal sequential LU factorization.
*

2
gtsam/3rdparty/Eigen/Eigen/src/Core/CwiseUnaryView.h vendored
View File

@ -38,7 +38,7 @@ struct traits<CwiseUnaryView<ViewOp, MatrixType> >
typedef typename remove_all<MatrixTypeNested>::type _MatrixTypeNested;
enum {
Flags = (traits<_MatrixTypeNested>::Flags & (HereditaryBits | LvalueBit | LinearAccessBit | DirectAccessBit)),
CoeffReadCost = traits<_MatrixTypeNested>::CoeffReadCost + functor_traits<ViewOp>::Cost,
CoeffReadCost = EIGEN_ADD_COST(traits<_MatrixTypeNested>::CoeffReadCost, functor_traits<ViewOp>::Cost),
MatrixTypeInnerStride = inner_stride_at_compile_time<MatrixType>::ret,
// need to cast the sizeof's from size_t to int explicitly, otherwise:
// "error: no integral type can represent all of the enumerator values

2
gtsam/3rdparty/Eigen/Eigen/src/Core/DenseBase.h vendored
View File

@ -40,7 +40,7 @@ static inline void check_DenseIndex_is_signed() {
*/
template<typename Derived> class DenseBase
#ifndef EIGEN_PARSED_BY_DOXYGEN
: public internal::special_scalar_op_base<Derived,typename internal::traits<Derived>::Scalar,
: public internal::special_scalar_op_base<Derived, typename internal::traits<Derived>::Scalar,
typename NumTraits<typename internal::traits<Derived>::Scalar>::Real,
DenseCoeffsBase<Derived> >
#else

11
gtsam/3rdparty/Eigen/Eigen/src/Core/GeneralProduct.h vendored
View File

@ -425,15 +425,18 @@ template<> struct gemv_selector<OnTheRight,ColMajor,true>
ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(prod.lhs())
* RhsBlasTraits::extractScalarFactor(prod.rhs());
// make sure Dest is a compile-time vector type (bug 1166)
typedef typename conditional<Dest::IsVectorAtCompileTime, Dest, typename Dest::ColXpr>::type ActualDest;
enum {
// FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
// on, the other hand it is good for the cache to pack the vector anyways...
EvalToDestAtCompileTime = Dest::InnerStrideAtCompileTime==1,
EvalToDestAtCompileTime = (ActualDest::InnerStrideAtCompileTime==1),
ComplexByReal = (NumTraits<LhsScalar>::IsComplex) && (!NumTraits<RhsScalar>::IsComplex),
MightCannotUseDest = (Dest::InnerStrideAtCompileTime!=1) || ComplexByReal
MightCannotUseDest = (ActualDest::InnerStrideAtCompileTime!=1) || ComplexByReal
};
gemv_static_vector_if<ResScalar,Dest::SizeAtCompileTime,Dest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest;
gemv_static_vector_if<ResScalar,ActualDest::SizeAtCompileTime,ActualDest::MaxSizeAtCompileTime,MightCannotUseDest> static_dest;
bool alphaIsCompatible = (!ComplexByReal) || (numext::imag(actualAlpha)==RealScalar(0));
bool evalToDest = EvalToDestAtCompileTime && alphaIsCompatible;
@ -522,7 +525,7 @@ template<> struct gemv_selector<OnTheRight,RowMajor,true>
actualLhs.rows(), actualLhs.cols(),
actualLhs.data(), actualLhs.outerStride(),
actualRhsPtr, 1,
dest.data(), dest.innerStride(),
dest.data(), dest.col(0).innerStride(), //NOTE if dest is not a vector at compile-time, then dest.innerStride() might be wrong. (bug 1166)
actualAlpha);
}
};

4
gtsam/3rdparty/Eigen/Eigen/src/Core/MapBase.h vendored
View File

@ -149,6 +149,10 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
checkSanity();
}
#ifdef EIGEN_MAPBASE_PLUGIN
#include EIGEN_MAPBASE_PLUGIN
#endif
protected:
void checkSanity() const

6
gtsam/3rdparty/Eigen/Eigen/src/Core/MathFunctions.h vendored
View File

@ -707,21 +707,21 @@ struct scalar_fuzzy_impl : scalar_fuzzy_default_impl<Scalar, NumTraits<Scalar>::
template<typename Scalar, typename OtherScalar>
inline bool isMuchSmallerThan(const Scalar& x, const OtherScalar& y,
typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision())
const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
{
return scalar_fuzzy_impl<Scalar>::template isMuchSmallerThan<OtherScalar>(x, y, precision);
}
template<typename Scalar>
inline bool isApprox(const Scalar& x, const Scalar& y,
typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision())
const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
{
return scalar_fuzzy_impl<Scalar>::isApprox(x, y, precision);
}
template<typename Scalar>
inline bool isApproxOrLessThan(const Scalar& x, const Scalar& y,
typename NumTraits<Scalar>::Real precision = NumTraits<Scalar>::dummy_precision())
const typename NumTraits<Scalar>::Real &precision = NumTraits<Scalar>::dummy_precision())
{
return scalar_fuzzy_impl<Scalar>::isApproxOrLessThan(x, y, precision);
}

8
gtsam/3rdparty/Eigen/Eigen/src/Core/SolveTriangular.h vendored
View File

@ -116,17 +116,17 @@ template<typename Lhs, typename Rhs, int Mode, int Index, int Size>
struct triangular_solver_unroller<Lhs,Rhs,Mode,Index,Size,false> {
enum {
IsLower = ((Mode&Lower)==Lower),
I = IsLower ? Index : Size - Index - 1,
S = IsLower ? 0 : I+1
RowIndex = IsLower ? Index : Size - Index - 1,
S = IsLower ? 0 : RowIndex+1
};
static void run(const Lhs& lhs, Rhs& rhs)
{
if (Index>0)
rhs.coeffRef(I) -= lhs.row(I).template segment<Index>(S).transpose()
rhs.coeffRef(RowIndex) -= lhs.row(RowIndex).template segment<Index>(S).transpose()
.cwiseProduct(rhs.template segment<Index>(S)).sum();
if(!(Mode & UnitDiag))
rhs.coeffRef(I) /= lhs.coeff(I,I);
rhs.coeffRef(RowIndex) /= lhs.coeff(RowIndex,RowIndex);
triangular_solver_unroller<Lhs,Rhs,Mode,Index+1,Size>::run(lhs,rhs);
}

7
gtsam/3rdparty/Eigen/Eigen/src/Core/Visitor.h vendored
View File

@ -76,14 +76,17 @@ template<typename Derived>
template<typename Visitor>
void DenseBase<Derived>::visit(Visitor& visitor) const
{
typedef typename internal::remove_all<typename Derived::Nested>::type ThisNested;
typename Derived::Nested thisNested(derived());
enum { unroll = SizeAtCompileTime != Dynamic
&& CoeffReadCost != Dynamic
&& (SizeAtCompileTime == 1 || internal::functor_traits<Visitor>::Cost != Dynamic)
&& SizeAtCompileTime * CoeffReadCost + (SizeAtCompileTime-1) * internal::functor_traits<Visitor>::Cost
<= EIGEN_UNROLLING_LIMIT };
return internal::visitor_impl<Visitor, Derived,
return internal::visitor_impl<Visitor, ThisNested,
unroll ? int(SizeAtCompileTime) : Dynamic
>::run(derived(), visitor);
>::run(thisNested, visitor);
}
namespace internal {

44
gtsam/3rdparty/Eigen/Eigen/src/Core/arch/SSE/PacketMath.h vendored
View File

@ -235,63 +235,27 @@ template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int* from) { E
return _mm_loadu_ps(from);
#endif
}
template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_pd(from); }
template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm_loadu_si128(reinterpret_cast<const Packet4i*>(from)); }
#else
// Fast unaligned loads. Note that here we cannot directly use intrinsics: this would
// require pointer casting to incompatible pointer types and leads to invalid code
// because of the strict aliasing rule. The "dummy" stuff are required to enforce
// a correct instruction dependency.
// TODO: do the same for MSVC (ICC is compatible)
// NOTE: with the code below, MSVC's compiler crashes!
#if defined(__GNUC__) && defined(__i386__)
// bug 195: gcc/i386 emits weird x87 fldl/fstpl instructions for _mm_load_sd
#define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1
#elif defined(__clang__)
// bug 201: Segfaults in __mm_loadh_pd with clang 2.8
#define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 1
#else
#define EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS 0
#endif
template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from)
{
EIGEN_DEBUG_UNALIGNED_LOAD
#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
return _mm_loadu_ps(from);
#else
__m128d res;
res = _mm_load_sd((const double*)(from)) ;
res = _mm_loadh_pd(res, (const double*)(from+2)) ;
return _mm_castpd_ps(res);
#endif
}
#endif
template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from)
{
EIGEN_DEBUG_UNALIGNED_LOAD
#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
return _mm_loadu_pd(from);
#else
__m128d res;
res = _mm_load_sd(from) ;
res = _mm_loadh_pd(res,from+1);
return res;
#endif
}
template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int* from)
{
EIGEN_DEBUG_UNALIGNED_LOAD
#if EIGEN_AVOID_CUSTOM_UNALIGNED_LOADS
return _mm_loadu_si128(reinterpret_cast<const Packet4i*>(from));
#else
__m128d res;
res = _mm_load_sd((const double*)(from)) ;
res = _mm_loadh_pd(res, (const double*)(from+2)) ;
return _mm_castpd_si128(res);
#endif
return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
}
#endif
template<> EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float* from)
{

8
gtsam/3rdparty/Eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h vendored
View File

@ -140,8 +140,10 @@ static void run(Index rows, Index cols, Index depth,
// Release all the sub blocks B'_j of B' for the current thread,
// i.e., we simply decrement the number of users by 1
for(Index j=0; j<threads; ++j)
{
#pragma omp atomic
--(info[j].users);
info[j].users -= 1;
}
}
}
else
@ -390,13 +392,17 @@ class GeneralProduct<Lhs, Rhs, GemmProduct>
GeneralProduct(const Lhs& lhs, const Rhs& rhs) : Base(lhs,rhs)
{
#if !(defined(EIGEN_NO_STATIC_ASSERT) && defined(EIGEN_NO_DEBUG))
typedef internal::scalar_product_op<LhsScalar,RhsScalar> BinOp;
EIGEN_CHECK_BINARY_COMPATIBILIY(BinOp,LhsScalar,RhsScalar);
#endif
}
template<typename Dest> void scaleAndAddTo(Dest& dst, const Scalar& alpha) const
{
eigen_assert(dst.rows()==m_lhs.rows() && dst.cols()==m_rhs.cols());
if(m_lhs.cols()==0 || m_lhs.rows()==0 || m_rhs.cols()==0)
return;
typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(m_lhs);
typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(m_rhs);

4
gtsam/3rdparty/Eigen/Eigen/src/Core/products/TriangularSolverMatrix.h vendored
View File

@ -115,8 +115,9 @@ EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conju
{
// TODO write a small kernel handling this (can be shared with trsv)
Index i = IsLower ? k2+k1+k : k2-k1-k-1;
Index s = IsLower ? k2+k1 : i+1;
Index rs = actualPanelWidth - k - 1; // remaining size
Index s = TriStorageOrder==RowMajor ? (IsLower ? k2+k1 : i+1)
: IsLower ? i+1 : i-rs;
Scalar a = (Mode & UnitDiag) ? Scalar(1) : Scalar(1)/conj(tri(i,i));
for (Index j=j2; j<j2+actual_cols; ++j)
@ -133,7 +134,6 @@ EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conju
}
else
{
Index s = IsLower ? i+1 : i-rs;
Scalar b = (other(i,j) *= a);
Scalar* r = &other(s,j);
const Scalar* l = &tri(s,i);

287
gtsam/3rdparty/Eigen/Eigen/src/Core/util/Macros.h vendored
View File

@ -13,23 +13,292 @@
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 7
#define EIGEN_MINOR_VERSION 8
#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
(EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
EIGEN_MINOR_VERSION>=z))))
// Compiler identification, EIGEN_COMP_*
/// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
#ifdef __GNUC__
#define EIGEN_COMP_GNUC 1
#else
#define EIGEN_COMP_GNUC 0
#endif
/// \internal EIGEN_COMP_CLANG set to 1 if the compiler is clang (alias for __clang__)
#if defined(__clang__)
#define EIGEN_COMP_CLANG 1
#else
#define EIGEN_COMP_CLANG 0
#endif
/// \internal EIGEN_COMP_LLVM set to 1 if the compiler backend is llvm
#if defined(__llvm__)
#define EIGEN_COMP_LLVM 1
#else
#define EIGEN_COMP_LLVM 0
#endif
/// \internal EIGEN_COMP_ICC set to __INTEL_COMPILER if the compiler is Intel compiler, 0 otherwise
#if defined(__INTEL_COMPILER)
#define EIGEN_COMP_ICC __INTEL_COMPILER
#else
#define EIGEN_COMP_ICC 0
#endif
/// \internal EIGEN_COMP_MINGW set to 1 if the compiler is mingw
#if defined(__MINGW32__)
#define EIGEN_COMP_MINGW 1
#else
#define EIGEN_COMP_MINGW 0
#endif
/// \internal EIGEN_COMP_SUNCC set to 1 if the compiler is Solaris Studio
#if defined(__SUNPRO_CC)
#define EIGEN_COMP_SUNCC 1
#else
#define EIGEN_COMP_SUNCC 0
#endif
/// \internal EIGEN_COMP_MSVC set to _MSC_VER if the compiler is Microsoft Visual C++, 0 otherwise.
#if defined(_MSC_VER)
#define EIGEN_COMP_MSVC _MSC_VER
#else
#define EIGEN_COMP_MSVC 0
#endif
/// \internal EIGEN_COMP_MSVC_STRICT set to 1 if the compiler is really Microsoft Visual C++ and not ,e.g., ICC
#if EIGEN_COMP_MSVC && !(EIGEN_COMP_ICC)
#define EIGEN_COMP_MSVC_STRICT _MSC_VER
#else
#define EIGEN_COMP_MSVC_STRICT 0
#endif
/// \internal EIGEN_COMP_IBM set to 1 if the compiler is IBM XL C++
#if defined(__IBMCPP__) || defined(__xlc__)
#define EIGEN_COMP_IBM 1
#else
#define EIGEN_COMP_IBM 0
#endif
/// \internal EIGEN_COMP_PGI set to 1 if the compiler is Portland Group Compiler
#if defined(__PGI)
#define EIGEN_COMP_PGI 1
#else
#define EIGEN_COMP_PGI 0
#endif
/// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler
#if defined(__CC_ARM) || defined(__ARMCC_VERSION)
#define EIGEN_COMP_ARM 1
#else
#define EIGEN_COMP_ARM 0
#endif
/// \internal EIGEN_GNUC_STRICT set to 1 if the compiler is really GCC and not a compatible compiler (e.g., ICC, clang, mingw, etc.)
#if EIGEN_COMP_GNUC && !(EIGEN_COMP_CLANG || EIGEN_COMP_ICC || EIGEN_COMP_MINGW || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM )
#define EIGEN_COMP_GNUC_STRICT 1
#else
#define EIGEN_COMP_GNUC_STRICT 0
#endif
#if EIGEN_COMP_GNUC
#define EIGEN_GNUC_AT_LEAST(x,y) ((__GNUC__==x && __GNUC_MINOR__>=y) || __GNUC__>x)
#define EIGEN_GNUC_AT_MOST(x,y) ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__<x)
#define EIGEN_GNUC_AT(x,y) ( __GNUC__==x && __GNUC_MINOR__==y )
#else
#define EIGEN_GNUC_AT_LEAST(x,y) 0
#define EIGEN_GNUC_AT_MOST(x,y) 0
#define EIGEN_GNUC_AT(x,y) 0
#endif
#ifdef __GNUC__
#define EIGEN_GNUC_AT_MOST(x,y) ((__GNUC__==x && __GNUC_MINOR__<=y) || __GNUC__<x)
// FIXME: could probably be removed as we do not support gcc 3.x anymore
#if EIGEN_COMP_GNUC && (__GNUC__ <= 3)
#define EIGEN_GCC3_OR_OLDER 1
#else
#define EIGEN_GNUC_AT_MOST(x,y) 0
#define EIGEN_GCC3_OR_OLDER 0
#endif
// Architecture identification, EIGEN_ARCH_*
#if defined(__x86_64__) || defined(_M_X64) || defined(__amd64)
#define EIGEN_ARCH_x86_64 1
#else
#define EIGEN_ARCH_x86_64 0
#endif
#if defined(__i386__) || defined(_M_IX86) || defined(_X86_) || defined(__i386)
#define EIGEN_ARCH_i386 1
#else
#define EIGEN_ARCH_i386 0
#endif
#if EIGEN_ARCH_x86_64 || EIGEN_ARCH_i386
#define EIGEN_ARCH_i386_OR_x86_64 1
#else
#define EIGEN_ARCH_i386_OR_x86_64 0
#endif
/// \internal EIGEN_ARCH_ARM set to 1 if the architecture is ARM
#if defined(__arm__)
#define EIGEN_ARCH_ARM 1
#else
#define EIGEN_ARCH_ARM 0
#endif
/// \internal EIGEN_ARCH_ARM64 set to 1 if the architecture is ARM64
#if defined(__aarch64__)
#define EIGEN_ARCH_ARM64 1
#else
#define EIGEN_ARCH_ARM64 0
#endif
#if EIGEN_ARCH_ARM || EIGEN_ARCH_ARM64
#define EIGEN_ARCH_ARM_OR_ARM64 1
#else
#define EIGEN_ARCH_ARM_OR_ARM64 0
#endif
/// \internal EIGEN_ARCH_MIPS set to 1 if the architecture is MIPS
#if defined(__mips__) || defined(__mips)
#define EIGEN_ARCH_MIPS 1
#else
#define EIGEN_ARCH_MIPS 0
#endif
/// \internal EIGEN_ARCH_SPARC set to 1 if the architecture is SPARC
#if defined(__sparc__) || defined(__sparc)
#define EIGEN_ARCH_SPARC 1
#else
#define EIGEN_ARCH_SPARC 0
#endif
/// \internal EIGEN_ARCH_IA64 set to 1 if the architecture is Intel Itanium
#if defined(__ia64__)
#define EIGEN_ARCH_IA64 1
#else
#define EIGEN_ARCH_IA64 0
#endif
/// \internal EIGEN_ARCH_PPC set to 1 if the architecture is PowerPC
#if defined(__powerpc__) || defined(__ppc__) || defined(_M_PPC)
#define EIGEN_ARCH_PPC 1
#else
#define EIGEN_ARCH_PPC 0
#endif
// Operating system identification, EIGEN_OS_*
/// \internal EIGEN_OS_UNIX set to 1 if the OS is a unix variant
#if defined(__unix__) || defined(__unix)
#define EIGEN_OS_UNIX 1
#else
#define EIGEN_OS_UNIX 0
#endif
/// \internal EIGEN_OS_LINUX set to 1 if the OS is based on Linux kernel
#if defined(__linux__)
#define EIGEN_OS_LINUX 1
#else
#define EIGEN_OS_LINUX 0
#endif
/// \internal EIGEN_OS_ANDROID set to 1 if the OS is Android
// note: ANDROID is defined when using ndk_build, __ANDROID__ is defined when using a standalone toolchain.
#if defined(__ANDROID__) || defined(ANDROID)
#define EIGEN_OS_ANDROID 1
#else
#define EIGEN_OS_ANDROID 0
#endif
/// \internal EIGEN_OS_GNULINUX set to 1 if the OS is GNU Linux and not Linux-based OS (e.g., not android)
#if defined(__gnu_linux__) && !(EIGEN_OS_ANDROID)
#define EIGEN_OS_GNULINUX 1
#else
#define EIGEN_OS_GNULINUX 0
#endif
/// \internal EIGEN_OS_BSD set to 1 if the OS is a BSD variant
#if defined(__FreeBSD__) || defined(__NetBSD__) || defined(__OpenBSD__) || defined(__bsdi__) || defined(__DragonFly__)
#define EIGEN_OS_BSD 1
#else
#define EIGEN_OS_BSD 0
#endif
/// \internal EIGEN_OS_MAC set to 1 if the OS is MacOS
#if defined(__APPLE__)
#define EIGEN_OS_MAC 1
#else
#define EIGEN_OS_MAC 0
#endif
/// \internal EIGEN_OS_QNX set to 1 if the OS is QNX
#if defined(__QNX__)
#define EIGEN_OS_QNX 1
#else
#define EIGEN_OS_QNX 0
#endif
/// \internal EIGEN_OS_WIN set to 1 if the OS is Windows based
#if defined(_WIN32)
#define EIGEN_OS_WIN 1
#else
#define EIGEN_OS_WIN 0
#endif
/// \internal EIGEN_OS_WIN64 set to 1 if the OS is Windows 64bits
#if defined(_WIN64)
#define EIGEN_OS_WIN64 1
#else
#define EIGEN_OS_WIN64 0
#endif
/// \internal EIGEN_OS_WINCE set to 1 if the OS is Windows CE
#if defined(_WIN32_WCE)
#define EIGEN_OS_WINCE 1
#else
#define EIGEN_OS_WINCE 0
#endif
/// \internal EIGEN_OS_CYGWIN set to 1 if the OS is Windows/Cygwin
#if defined(__CYGWIN__)
#define EIGEN_OS_CYGWIN 1
#else
#define EIGEN_OS_CYGWIN 0
#endif
/// \internal EIGEN_OS_WIN_STRICT set to 1 if the OS is really Windows and not some variants
#if EIGEN_OS_WIN && !( EIGEN_OS_WINCE || EIGEN_OS_CYGWIN )
#define EIGEN_OS_WIN_STRICT 1
#else
#define EIGEN_OS_WIN_STRICT 0
#endif
/// \internal EIGEN_OS_SUN set to 1 if the OS is SUN
#if (defined(sun) || defined(__sun)) && !(defined(__SVR4) || defined(__svr4__))
#define EIGEN_OS_SUN 1
#else
#define EIGEN_OS_SUN 0
#endif
/// \internal EIGEN_OS_SOLARIS set to 1 if the OS is Solaris
#if (defined(sun) || defined(__sun)) && (defined(__SVR4) || defined(__svr4__))
#define EIGEN_OS_SOLARIS 1
#else
#define EIGEN_OS_SOLARIS 0
#endif
#if EIGEN_GNUC_AT_MOST(4,3) && !defined(__clang__)
// see bug 89
#define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 0
@ -37,12 +306,6 @@
#define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 1
#endif
#if defined(__GNUC__) && (__GNUC__ <= 3)
#define EIGEN_GCC3_OR_OLDER 1
#else
#define EIGEN_GCC3_OR_OLDER 0
#endif
// 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable
// 16 byte alignment on all platforms where vectorization might be enabled. In theory we could always
// enable alignment, but it can be a cause of problems on some platforms, so we just disable it in
@ -104,7 +367,7 @@
// Do we support r-value references?
#if (__has_feature(cxx_rvalue_references) || \
defined(__GXX_EXPERIMENTAL_CXX0X__) || \
(defined(__cplusplus) && __cplusplus >= 201103L) || \
(defined(_MSC_VER) && _MSC_VER >= 1600))
#define EIGEN_HAVE_RVALUE_REFERENCES
#endif

2
gtsam/3rdparty/Eigen/Eigen/src/Core/util/StaticAssert.h vendored
View File

@ -26,7 +26,7 @@
#ifndef EIGEN_NO_STATIC_ASSERT
#if defined(__GXX_EXPERIMENTAL_CXX0X__) || (defined(_MSC_VER) && (_MSC_VER >= 1600))
#if __has_feature(cxx_static_assert) || (defined(__cplusplus) && __cplusplus >= 201103L) || (EIGEN_COMP_MSVC >= 1600)
// if native static_assert is enabled, let's use it
#define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);

1
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/ComplexSchur_MKL.h vendored
View File

@ -45,7 +45,6 @@ ComplexSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >& \
ComplexSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >::compute(const Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW>& matrix, bool computeU) \
{ \
typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> MatrixType; \
typedef MatrixType::Scalar Scalar; \
typedef MatrixType::RealScalar RealScalar; \
typedef std::complex<RealScalar> ComplexScalar; \
\

4
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealSchur_MKL.h vendored
View File

@ -44,10 +44,6 @@ template<> inline \
RealSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >& \
RealSchur<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> >::compute(const Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW>& matrix, bool computeU) \
{ \
typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW> MatrixType; \
typedef MatrixType::Scalar Scalar; \
typedef MatrixType::RealScalar RealScalar; \
\
eigen_assert(matrix.cols() == matrix.rows()); \
\
lapack_int n = matrix.cols(), sdim, info; \

7
gtsam/3rdparty/Eigen/Eigen/src/Geometry/AngleAxis.h vendored
View File

@ -83,10 +83,17 @@ public:
template<typename Derived>
inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
/** \returns the value of the rotation angle in radian */
Scalar angle() const { return m_angle; }
/** \returns a read-write reference to the stored angle in radian */
Scalar& angle() { return m_angle; }
/** \returns the rotation axis */
const Vector3& axis() const { return m_axis; }
/** \returns a read-write reference to the stored rotation axis.
*
* \warning The rotation axis must remain a \b unit vector.
*/
Vector3& axis() { return m_axis; }
/** Concatenates two rotations */

2
gtsam/3rdparty/Eigen/Eigen/src/Geometry/ParametrizedLine.h vendored
View File

@ -129,7 +129,7 @@ public:
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const ParametrizedLine& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
bool isApprox(const ParametrizedLine& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return m_origin.isApprox(other.m_origin, prec) && m_direction.isApprox(other.m_direction, prec); }
protected:

49
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Transform.h vendored
View File

@ -102,15 +102,15 @@ template<int Mode> struct transform_make_affine;
*
* However, unlike a plain matrix, the Transform class provides many features
* simplifying both its assembly and usage. In particular, it can be composed
* with any other transformations (Transform,Translation,RotationBase,Matrix)
* with any other transformations (Transform,Translation,RotationBase,DiagonalMatrix)
* and can be directly used to transform implicit homogeneous vectors. All these
* operations are handled via the operator*. For the composition of transformations,
* its principle consists to first convert the right/left hand sides of the product
* to a compatible (Dim+1)^2 matrix and then perform a pure matrix product.
* Of course, internally, operator* tries to perform the minimal number of operations
* according to the nature of each terms. Likewise, when applying the transform
* to non homogeneous vectors, the latters are automatically promoted to homogeneous
* one before doing the matrix product. The convertions to homogeneous representations
* to points, the latters are automatically promoted to homogeneous vectors
* before doing the matrix product. The conventions to homogeneous representations
* are performed as follow:
*
* \b Translation t (Dim)x(1):
@ -124,7 +124,7 @@ template<int Mode> struct transform_make_affine;
* R & 0\\
* 0\,...\,0 & 1
* \end{array} \right) \f$
*
*<!--
* \b Linear \b Matrix L (Dim)x(Dim):
* \f$ \left( \begin{array}{cc}
* L & 0\\
@ -136,14 +136,20 @@ template<int Mode> struct transform_make_affine;
* A\\
* 0\,...\,0\,1
* \end{array} \right) \f$
*-->
* \b Scaling \b DiagonalMatrix S (Dim)x(Dim):
* \f$ \left( \begin{array}{cc}
* S & 0\\
* 0\,...\,0 & 1
* \end{array} \right) \f$
*
* \b Column \b vector v (Dim)x(1):
* \b Column \b point v (Dim)x(1):
* \f$ \left( \begin{array}{c}
* v\\
* 1
* \end{array} \right) \f$
*
* \b Set \b of \b column \b vectors V1...Vn (Dim)x(n):
* \b Set \b of \b column \b points V1...Vn (Dim)x(n):
* \f$ \left( \begin{array}{ccc}
* v_1 & ... & v_n\\
* 1 & ... & 1
@ -384,26 +390,39 @@ public:
/** \returns a writable expression of the translation vector of the transformation */
inline TranslationPart translation() { return TranslationPart(m_matrix,0,Dim); }
/** \returns an expression of the product between the transform \c *this and a matrix expression \a other
/** \returns an expression of the product between the transform \c *this and a matrix expression \a other.
*
* The right hand side \a other might be either:
* \li a vector of size Dim,
* The right-hand-side \a other can be either:
* \li an homogeneous vector of size Dim+1,
* \li a set of vectors of size Dim x Dynamic,
* \li a set of homogeneous vectors of size Dim+1 x Dynamic,
* \li a linear transformation matrix of size Dim x Dim,
* \li an affine transformation matrix of size Dim x Dim+1,
* \li a set of homogeneous vectors of size Dim+1 x N,
* \li a transformation matrix of size Dim+1 x Dim+1.
*
* Moreover, if \c *this represents an affine transformation (i.e., Mode!=Projective), then \a other can also be:
* \li a point of size Dim (computes: \code this->linear() * other + this->translation()\endcode),
* \li a set of N points as a Dim x N matrix (computes: \code (this->linear() * other).colwise() + this->translation()\endcode),
*
* In all cases, the return type is a matrix or vector of same sizes as the right-hand-side \a other.
*
* If you want to interpret \a other as a linear or affine transformation, then first convert it to a Transform<> type,
* or do your own cooking.
*
* Finally, if you want to apply Affine transformations to vectors, then explicitly apply the linear part only:
* \code
* Affine3f A;
* Vector3f v1, v2;
* v2 = A.linear() * v1;
* \endcode
*
*/
// note: this function is defined here because some compilers cannot find the respective declaration
template<typename OtherDerived>
EIGEN_STRONG_INLINE const typename internal::transform_right_product_impl<Transform, OtherDerived>::ResultType
EIGEN_STRONG_INLINE const typename OtherDerived::PlainObject
operator * (const EigenBase<OtherDerived> &other) const
{ return internal::transform_right_product_impl<Transform, OtherDerived>::run(*this,other.derived()); }
/** \returns the product expression of a transformation matrix \a a times a transform \a b
*
* The left hand side \a other might be either:
* The left hand side \a other can be either:
* \li a linear transformation matrix of size Dim x Dim,
* \li an affine transformation matrix of size Dim x Dim+1,
* \li a general transformation matrix of size Dim+1 x Dim+1.

2
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Translation.h vendored
View File

@ -162,7 +162,7 @@ public:
* determined by \a prec.
*
* \sa MatrixBase::isApprox() */
bool isApprox(const Translation& other, typename NumTraits<Scalar>::Real prec = NumTraits<Scalar>::dummy_precision()) const
bool isApprox(const Translation& other, const typename NumTraits<Scalar>::Real& prec = NumTraits<Scalar>::dummy_precision()) const
{ return m_coeffs.isApprox(other.m_coeffs, prec); }
};

2
gtsam/3rdparty/Eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h vendored
View File

@ -139,6 +139,8 @@ struct traits<ConjugateGradient<_MatrixType,_UpLo,_Preconditioner> >
* By default the iterations start with x=0 as an initial guess of the solution.
* One can control the start using the solveWithGuess() method.
*
* ConjugateGradient can also be used in a matrix-free context, see the following \link MatrixfreeSolverExample example \endlink.
*
* \sa class SimplicialCholesky, DiagonalPreconditioner, IdentityPreconditioner
*/
template< typename _MatrixType, int _UpLo, typename _Preconditioner>

2
gtsam/3rdparty/Eigen/Eigen/src/LU/FullPivLU.h vendored
View File

@ -688,7 +688,7 @@ struct solve_retval<FullPivLU<_MatrixType>, Rhs>
*/
const Index rows = dec().rows(), cols = dec().cols(),
nonzero_pivots = dec().nonzeroPivots();
nonzero_pivots = dec().rank();
eigen_assert(rhs().rows() == rows);
const Index smalldim = (std::min)(rows, cols);

2
gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Amd.h vendored
View File

@ -8,7 +8,7 @@
NOTE: this routine has been adapted from the CSparse library:
Copyright (c) 2006, Timothy A. Davis.
http://www.cise.ufl.edu/research/sparse/CSparse
http://www.suitesparse.com
CSparse is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public

31
gtsam/3rdparty/Eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h vendored
View File

@ -41,12 +41,8 @@
//
// The colamd/symamd library is available at
//
// http://www.cise.ufl.edu/research/sparse/colamd/
// http://www.suitesparse.com
// This is the http://www.cise.ufl.edu/research/sparse/colamd/colamd.h
// file. It is required by the colamd.c, colamdmex.c, and symamdmex.c
// files, and by any C code that calls the routines whose prototypes are
// listed below, or that uses the colamd/symamd definitions listed below.
#ifndef EIGEN_COLAMD_H
#define EIGEN_COLAMD_H
@ -102,9 +98,6 @@ namespace internal {
/* === Definitions ========================================================== */
/* ========================================================================== */
#define COLAMD_MAX(a,b) (((a) > (b)) ? (a) : (b))
#define COLAMD_MIN(a,b) (((a) < (b)) ? (a) : (b))
#define ONES_COMPLEMENT(r) (-(r)-1)
/* -------------------------------------------------------------------------- */
@ -516,7 +509,7 @@ static Index init_rows_cols /* returns true if OK, or false otherwise */
Col [col].start = p [col] ;
Col [col].length = p [col+1] - p [col] ;
if (Col [col].length < 0)
if ((Col [col].length) < 0) // extra parentheses to work-around gcc bug 10200
{
/* column pointers must be non-decreasing */
stats [COLAMD_STATUS] = COLAMD_ERROR_col_length_negative ;
@ -739,8 +732,8 @@ static void init_scoring
/* === Extract knobs ==================================================== */
dense_row_count = COLAMD_MAX (0, COLAMD_MIN (knobs [COLAMD_DENSE_ROW] * n_col, n_col)) ;
dense_col_count = COLAMD_MAX (0, COLAMD_MIN (knobs [COLAMD_DENSE_COL] * n_row, n_row)) ;
dense_row_count = std::max<Index>(0, (std::min)(Index(knobs [COLAMD_DENSE_ROW] * n_col), n_col)) ;
dense_col_count = std::max<Index>(0, (std::min)(Index(knobs [COLAMD_DENSE_COL] * n_row), n_row)) ;
COLAMD_DEBUG1 (("colamd: densecount: %d %d\n", dense_row_count, dense_col_count)) ;
max_deg = 0 ;
n_col2 = n_col ;
@ -804,7 +797,7 @@ static void init_scoring
else
{
/* keep track of max degree of remaining rows */
max_deg = COLAMD_MAX (max_deg, deg) ;
max_deg = (std::max)(max_deg, deg) ;
}
}
COLAMD_DEBUG1 (("colamd: Dense and null rows killed: %d\n", n_row - n_row2)) ;
@ -842,7 +835,7 @@ static void init_scoring
/* add row's external degree */
score += Row [row].shared1.degree - 1 ;
/* guard against integer overflow */
score = COLAMD_MIN (score, n_col) ;
score = (std::min)(score, n_col) ;
}
/* determine pruned column length */
col_length = (Index) (new_cp - &A [Col [c].start]) ;
@ -914,7 +907,7 @@ static void init_scoring
head [score] = c ;
/* see if this score is less than current min */
min_score = COLAMD_MIN (min_score, score) ;
min_score = (std::min)(min_score, score) ;
}
@ -1040,7 +1033,7 @@ static Index find_ordering /* return the number of garbage collections */
/* === Garbage_collection, if necessary ============================= */
needed_memory = COLAMD_MIN (pivot_col_score, n_col - k) ;
needed_memory = (std::min)(pivot_col_score, n_col - k) ;
if (pfree + needed_memory >= Alen)
{
pfree = Eigen::internal::garbage_collection (n_row, n_col, Row, Col, A, &A [pfree]) ;
@ -1099,7 +1092,7 @@ static Index find_ordering /* return the number of garbage collections */
/* clear tag on pivot column */
Col [pivot_col].shared1.thickness = pivot_col_thickness ;
max_deg = COLAMD_MAX (max_deg, pivot_row_degree) ;
max_deg = (std::max)(max_deg, pivot_row_degree) ;
/* === Kill all rows used to construct pivot row ==================== */
@ -1273,7 +1266,7 @@ static Index find_ordering /* return the number of garbage collections */
/* add set difference */
cur_score += row_mark - tag_mark ;
/* integer overflow... */
cur_score = COLAMD_MIN (cur_score, n_col) ;
cur_score = (std::min)(cur_score, n_col) ;
}
/* recompute the column's length */
@ -1386,7 +1379,7 @@ static Index find_ordering /* return the number of garbage collections */
cur_score -= Col [col].shared1.thickness ;
/* make sure score is less or equal than the max score */
cur_score = COLAMD_MIN (cur_score, max_score) ;
cur_score = (std::min)(cur_score, max_score) ;
COLAMD_ASSERT (cur_score >= 0) ;
/* store updated score */
@ -1409,7 +1402,7 @@ static Index find_ordering /* return the number of garbage collections */
head [cur_score] = col ;
/* see if this score is less than current min */
min_score = COLAMD_MIN (min_score, cur_score) ;
min_score = (std::min)(min_score, cur_score) ;
}

1
gtsam/3rdparty/Eigen/Eigen/src/QR/ColPivHouseholderQR_MKL.h vendored
View File

@ -49,7 +49,6 @@ ColPivHouseholderQR<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynami
{ \
using std::abs; \
typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic> MatrixType; \
typedef MatrixType::Scalar Scalar; \
typedef MatrixType::RealScalar RealScalar; \
Index rows = matrix.rows();\
Index cols = matrix.cols();\

2
gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD.h vendored
View File

@ -816,7 +816,7 @@ void JacobiSVD<MatrixType, QRPreconditioner>::allocate(Index rows, Index cols, u
if(m_cols>m_rows) m_qr_precond_morecols.allocate(*this);
if(m_rows>m_cols) m_qr_precond_morerows.allocate(*this);
if(m_cols!=m_cols) m_scaledMatrix.resize(rows,cols);
if(m_rows!=m_cols) m_scaledMatrix.resize(rows,cols);
}
template<typename MatrixType, int QRPreconditioner>

4
gtsam/3rdparty/Eigen/Eigen/src/SVD/JacobiSVD_MKL.h vendored
View File

@ -45,8 +45,8 @@ JacobiSVD<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>, ColPiv
JacobiSVD<Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>, ColPivHouseholderQRPreconditioner>::compute(const Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic>& matrix, unsigned int computationOptions) \
{ \
typedef Matrix<EIGTYPE, Dynamic, Dynamic, EIGCOLROW, Dynamic, Dynamic> MatrixType; \
typedef MatrixType::Scalar Scalar; \
typedef MatrixType::RealScalar RealScalar; \
/*typedef MatrixType::Scalar Scalar;*/ \
/*typedef MatrixType::RealScalar RealScalar;*/ \
allocate(matrix.rows(), matrix.cols(), computationOptions); \
\
/*const RealScalar precision = RealScalar(2) * NumTraits<Scalar>::epsilon();*/ \

6
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseBlock.h vendored
View File

@ -364,10 +364,11 @@ public:
protected:
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
typename SparseMatrixType::Nested m_matrix;
Index m_outerStart;
const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
};
//----------
@ -528,7 +529,8 @@ public:
const internal::variable_if_dynamic<Index, XprType::ColsAtCompileTime == 1 ? 0 : Dynamic> m_startCol;
const internal::variable_if_dynamic<Index, RowsAtCompileTime> m_blockRows;
const internal::variable_if_dynamic<Index, ColsAtCompileTime> m_blockCols;
private:
Index nonZeros() const;
};
} // end namespace Eigen

5
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h vendored
View File

@ -55,10 +55,9 @@ class CwiseBinaryOpImpl<BinaryOp, Lhs, Rhs, Sparse>
EIGEN_SPARSE_PUBLIC_INTERFACE(Derived)
CwiseBinaryOpImpl()
{
typedef typename internal::traits<Lhs>::StorageKind LhsStorageKind;
typedef typename internal::traits<Rhs>::StorageKind RhsStorageKind;
EIGEN_STATIC_ASSERT((
(!internal::is_same<LhsStorageKind,RhsStorageKind>::value)
(!internal::is_same<typename internal::traits<Lhs>::StorageKind,
typename internal::traits<Rhs>::StorageKind>::value)
|| ((Lhs::Flags&RowMajorBit) == (Rhs::Flags&RowMajorBit))),
THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH);
}

6
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseView.h vendored
View File

@ -35,9 +35,9 @@ class SparseView : public SparseMatrixBase<SparseView<MatrixType> >
public:
EIGEN_SPARSE_PUBLIC_INTERFACE(SparseView)
SparseView(const MatrixType& mat, const Scalar& m_reference = Scalar(0),
typename NumTraits<Scalar>::Real m_epsilon = NumTraits<Scalar>::dummy_precision()) :
m_matrix(mat), m_reference(m_reference), m_epsilon(m_epsilon) {}
explicit SparseView(const MatrixType& mat, const Scalar& reference = Scalar(0),
const RealScalar &epsilon = NumTraits<Scalar>::dummy_precision())
: m_matrix(mat), m_reference(reference), m_epsilon(epsilon) {}
class InnerIterator;

18
gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdDeque.h vendored
View File

@ -13,32 +13,24 @@
#include "details.h"
// Define the explicit instantiation (e.g. necessary for the Intel compiler)
#if defined(__INTEL_COMPILER) || defined(__GNUC__)
#define EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(...) template class std::deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> >;
#else
#define EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(...)
#endif
/**
* This section contains a convenience MACRO which allows an easy specialization of
* std::deque such that for data types with alignment issues the correct allocator
* is used automatically.
*/
#define EIGEN_DEFINE_STL_DEQUE_SPECIALIZATION(...) \
EIGEN_EXPLICIT_STL_DEQUE_INSTANTIATION(__VA_ARGS__) \
namespace std \
{ \
template<typename _Ay> \
class deque<__VA_ARGS__, _Ay> \
template<> \
class deque<__VA_ARGS__, std::allocator<__VA_ARGS__> > \
: public deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
{ \
typedef deque<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > deque_base; \
public: \
typedef __VA_ARGS__ value_type; \
typedef typename deque_base::allocator_type allocator_type; \
typedef typename deque_base::size_type size_type; \
typedef typename deque_base::iterator iterator; \
typedef deque_base::allocator_type allocator_type; \
typedef deque_base::size_type size_type; \
typedef deque_base::iterator iterator; \
explicit deque(const allocator_type& a = allocator_type()) : deque_base(a) {} \
template<typename InputIterator> \
deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : deque_base(first, last, a) {} \

18
gtsam/3rdparty/Eigen/Eigen/src/StlSupport/StdList.h vendored
View File

@ -12,32 +12,24 @@
#include "details.h"
// Define the explicit instantiation (e.g. necessary for the Intel compiler)
#if defined(__INTEL_COMPILER) || defined(__GNUC__)
#define EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(...) template class std::list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> >;
#else
#define EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(...)
#endif
/**
* This section contains a convenience MACRO which allows an easy specialization of
* std::list such that for data types with alignment issues the correct allocator
* is used automatically.
*/
#define EIGEN_DEFINE_STL_LIST_SPECIALIZATION(...) \
EIGEN_EXPLICIT_STL_LIST_INSTANTIATION(__VA_ARGS__) \
namespace std \
{ \
template<typename _Ay> \
class list<__VA_ARGS__, _Ay> \
template<> \
class list<__VA_ARGS__, std::allocator<__VA_ARGS__> > \
: public list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > \
{ \
typedef list<__VA_ARGS__, EIGEN_ALIGNED_ALLOCATOR<__VA_ARGS__> > list_base; \
public: \
typedef __VA_ARGS__ value_type; \
typedef typename list_base::allocator_type allocator_type; \
typedef typename list_base::size_type size_type; \
typedef typename list_base::iterator iterator; \
typedef list_base::allocator_type allocator_type; \
typedef list_base::size_type size_type; \
typedef list_base::iterator iterator; \
explicit list(const allocator_type& a = allocator_type()) : list_base(a) {} \
template<typename InputIterator> \
list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : list_base(first, last, a) {} \

1
gtsam/3rdparty/Eigen/doc/CMakeLists.txt vendored
View File

@ -89,6 +89,7 @@ add_dependencies(doc-unsupported-prerequisites unsupported_snippets unsupported_
add_custom_target(doc ALL
COMMAND doxygen
COMMAND doxygen Doxyfile-unsupported
COMMAND ${CMAKE_COMMAND} -E copy ${Eigen_BINARY_DIR}/doc/html/group__TopicUnalignedArrayAssert.html ${Eigen_BINARY_DIR}/doc/html/TopicUnalignedArrayAssert.html
COMMAND ${CMAKE_COMMAND} -E rename html eigen-doc
COMMAND ${CMAKE_COMMAND} -E remove eigen-doc/eigen-doc.tgz
COMMAND ${CMAKE_COMMAND} -E tar cfz eigen-doc.tgz eigen-doc

2
gtsam/3rdparty/Eigen/doc/Manual.dox vendored
View File

@ -121,6 +121,8 @@ namespace Eigen {
\ingroup Sparse_chapter */
/** \addtogroup TopicSparseSystems
\ingroup Sparse_chapter */
/** \addtogroup MatrixfreeSolverExample
\ingroup Sparse_chapter */
/** \addtogroup Sparse_Reference
\ingroup Sparse_chapter */

1
gtsam/3rdparty/Eigen/doc/PreprocessorDirectives.dox vendored
View File

@ -91,6 +91,7 @@ following macros are supported; none of them are defined by default.
- \b EIGEN_MATRIX_PLUGIN - filename of plugin for extending the Matrix class.
- \b EIGEN_MATRIXBASE_PLUGIN - filename of plugin for extending the MatrixBase class.
- \b EIGEN_PLAINOBJECTBASE_PLUGIN - filename of plugin for extending the PlainObjectBase class.
- \b EIGEN_MAPBASE_PLUGIN - filename of plugin for extending the MapBase class.
- \b EIGEN_QUATERNIONBASE_PLUGIN - filename of plugin for extending the QuaternionBase class.
- \b EIGEN_SPARSEMATRIX_PLUGIN - filename of plugin for extending the SparseMatrix class.
- \b EIGEN_SPARSEMATRIXBASE_PLUGIN - filename of plugin for extending the SparseMatrixBase class.

6
gtsam/3rdparty/Eigen/doc/SparseLinearSystems.dox vendored
View File

@ -35,17 +35,17 @@ They are summarized in the following table:
<td>Requires the <a href="http://pastix.gforge.inria.fr">PaStiX</a> package, \b CeCILL-C </td>
<td>optimized for tough problems and symmetric patterns</td></tr>
<tr><td>CholmodSupernodalLLT</td><td>\link CholmodSupport_Module CholmodSupport \endlink</td><td>Direct LLt factorization</td><td>SPD</td><td>Fill-in reducing, Leverage fast dense algebra</td>
<td>Requires the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">SuiteSparse</a> package, \b GPL </td>
<td>Requires the <a href="http://www.suitesparse.com">SuiteSparse</a> package, \b GPL </td>
<td></td></tr>
<tr><td>UmfPackLU</td><td>\link UmfPackSupport_Module UmfPackSupport \endlink</td><td>Direct LU factorization</td><td>Square</td><td>Fill-in reducing, Leverage fast dense algebra</td>
<td>Requires the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">SuiteSparse</a> package, \b GPL </td>
<td>Requires the <a href="http://www.suitesparse.com">SuiteSparse</a> package, \b GPL </td>
<td></td></tr>
<tr><td>SuperLU</td><td>\link SuperLUSupport_Module SuperLUSupport \endlink</td><td>Direct LU factorization</td><td>Square</td><td>Fill-in reducing, Leverage fast dense algebra</td>
<td>Requires the <a href="http://crd-legacy.lbl.gov/~xiaoye/SuperLU/">SuperLU</a> library, (BSD-like)</td>
<td></td></tr>
<tr><td>SPQR</td><td>\link SPQRSupport_Module SPQRSupport \endlink </td> <td> QR factorization </td>
<td> Any, rectangular</td><td>fill-in reducing, multithreaded, fast dense algebra</td>
<td> requires the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">SuiteSparse</a> package, \b GPL </td><td>recommended for linear least-squares problems, has a rank-revealing feature</tr>
<td> requires the <a href="http://www.suitesparse.com">SuiteSparse</a> package, \b GPL </td><td>recommended for linear least-squares problems, has a rank-revealing feature</tr>
</table>
Here \c SPD means symmetric positive definite.

62
gtsam/3rdparty/Eigen/doc/SparseQuickReference.dox vendored
View File

@ -21,7 +21,7 @@ i.e either row major or column major. The default is column major. Most arithmet
<td> Resize/Reserve</td>
<td>
\code
sm1.resize(m,n); //Change sm1 to a m x n matrix.
sm1.resize(m,n); // Change sm1 to a m x n matrix.
sm1.reserve(nnz); // Allocate room for nnz nonzeros elements.
\endcode
</td>
@ -151,10 +151,10 @@ It is easy to perform arithmetic operations on sparse matrices provided that the
<td> Permutation </td>
<td>
\code
perm.indices(); // Reference to the vector of indices
perm.indices(); // Reference to the vector of indices
sm1.twistedBy(perm); // Permute rows and columns
sm2 = sm1 * perm; //Permute the columns
sm2 = perm * sm1; // Permute the columns
sm2 = sm1 * perm; // Permute the columns
sm2 = perm * sm1; // Permute the columns
\endcode
</td>
<td>
@ -181,9 +181,9 @@ sm2 = perm * sm1; // Permute the columns
\section sparseotherops Other supported operations
<table class="manual">
<tr><th>Operations</th> <th> Code </th> <th> Notes</th> </tr>
<tr><th style="min-width:initial"> Code </th> <th> Notes</th> </tr>
<tr><td colspan="2">Sub-matrices</td></tr>
<tr>
<td>Sub-matrices</td>
<td>
\code
sm1.block(startRow, startCol, rows, cols);
@ -193,25 +193,31 @@ sm2 = perm * sm1; // Permute the columns
sm1.bottomLeftCorner( rows, cols);
sm1.bottomRightCorner( rows, cols);
\endcode
</td> <td> </td>
</td><td>
Contrary to dense matrices, here <strong>all these methods are read-only</strong>.\n
See \ref TutorialSparse_SubMatrices and below for read-write sub-matrices.
</td>
</tr>
<tr>
<td> Range </td>
<tr class="alt"><td colspan="2"> Range </td></tr>
<tr class="alt">
<td>
\code
sm1.innerVector(outer);
sm1.innerVectors(start, size);
sm1.leftCols(size);
sm2.rightCols(size);
sm1.middleRows(start, numRows);
sm1.middleCols(start, numCols);
sm1.col(j);
sm1.innerVector(outer); // RW
sm1.innerVectors(start, size); // RW
sm1.leftCols(size); // RW
sm2.rightCols(size); // RO because sm2 is row-major
sm1.middleRows(start, numRows); // RO becasue sm1 is column-major
sm1.middleCols(start, numCols); // RW
sm1.col(j); // RW
\endcode
</td>
<td>A inner vector is either a row (for row-major) or a column (for column-major). As stated earlier, the evaluation can be done in a matrix with different storage order </td>
<td>
A inner vector is either a row (for row-major) or a column (for column-major).\n
As stated earlier, for a read-write sub-matrix (RW), the evaluation can be done in a matrix with different storage order.
</td>
</tr>
<tr><td colspan="2"> Triangular and selfadjoint views</td></tr>
<tr>
<td> Triangular and selfadjoint views</td>
<td>
\code
sm2 = sm1.triangularview<Lower>();
@ -222,26 +228,30 @@ sm2 = perm * sm1; // Permute the columns
\code
\endcode </td>
</tr>
<tr>
<td>Triangular solve </td>
<tr class="alt"><td colspan="2">Triangular solve </td></tr>
<tr class="alt">
<td>
\code
dv2 = sm1.triangularView<Upper>().solve(dv1);
dv2 = sm1.topLeftCorner(size, size).triangularView<Lower>().solve(dv1);
dv2 = sm1.topLeftCorner(size, size)
.triangularView<Lower>().solve(dv1);
\endcode
</td>
<td> For general sparse solve, Use any suitable module described at \ref TopicSparseSystems </td>
</tr>
<tr><td colspan="2"> Low-level API</td></tr>
<tr>
<td> Low-level API</td>
<td>
\code
sm1.valuePtr(); // Pointer to the values
sm1.innerIndextr(); // Pointer to the indices.
sm1.outerIndexPtr(); //Pointer to the beginning of each inner vector
sm1.valuePtr(); // Pointer to the values
sm1.innerIndextr(); // Pointer to the indices.
sm1.outerIndexPtr(); // Pointer to the beginning of each inner vector
\endcode
</td>
<td> If the matrix is not in compressed form, makeCompressed() should be called before. Note that these functions are mostly provided for interoperability purposes with external libraries. A better access to the values of the matrix is done by using the InnerIterator class as described in \link TutorialSparse the Tutorial Sparse \endlink section</td>
<td>
If the matrix is not in compressed form, makeCompressed() should be called before.\n
Note that these functions are mostly provided for interoperability purposes with external libraries.\n
A better access to the values of the matrix is done by using the InnerIterator class as described in \link TutorialSparse the Tutorial Sparse \endlink section</td>
</tr>
</table>
*/

24
gtsam/3rdparty/Eigen/doc/TutorialSparse.dox vendored
View File

@ -241,11 +241,11 @@ In the following \em sm denotes a sparse matrix, \em sv a sparse vector, \em dm
sm1.real() sm1.imag() -sm1 0.5*sm1
sm1+sm2 sm1-sm2 sm1.cwiseProduct(sm2)
\endcode
However, a strong restriction is that the storage orders must match. For instance, in the following example:
However, <strong>a strong restriction is that the storage orders must match</strong>. For instance, in the following example:
\code
sm4 = sm1 + sm2 + sm3;
\endcode
sm1, sm2, and sm3 must all be row-major or all column major.
sm1, sm2, and sm3 must all be row-major or all column-major.
On the other hand, there is no restriction on the target matrix sm4.
For instance, this means that for computing \f$ A^T + A \f$, the matrix \f$ A^T \f$ must be evaluated into a temporary matrix of compatible storage order:
\code
@ -307,6 +307,26 @@ sm2 = sm1.transpose() * P;
\endcode
\subsection TutorialSparse_SubMatrices Block operations
Regarding read-access, sparse matrices expose the same API than for dense matrices to access to sub-matrices such as blocks, columns, and rows. See \ref TutorialBlockOperations for a detailed introduction.
However, for performance reasons, writing to a sub-sparse-matrix is much more limited, and currently only contiguous sets of columns (resp. rows) of a column-major (resp. row-major) SparseMatrix are writable. Moreover, this information has to be known at compile-time, leaving out methods such as <tt>block(...)</tt> and <tt>corner*(...)</tt>. The available API for write-access to a SparseMatrix are summarized below:
\code
SparseMatrix<double,ColMajor> sm1;
sm1.col(j) = ...;
sm1.leftCols(ncols) = ...;
sm1.middleCols(j,ncols) = ...;
sm1.rightCols(ncols) = ...;
SparseMatrix<double,RowMajor> sm2;
sm2.row(i) = ...;
sm2.topRows(nrows) = ...;
sm2.middleRows(i,nrows) = ...;
sm2.bottomRows(nrows) = ...;
\endcode
In addition, sparse matrices expose the SparseMatrixBase::innerVector() and SparseMatrixBase::innerVectors() methods, which are aliases to the col/middleCols methods for a column-major storage, and to the row/middleRows methods for a row-major storage.
\subsection TutorialSparse_TriangularSelfadjoint Triangular and selfadjoint views
Just as with dense matrices, the triangularView() function can be used to address a triangular part of the matrix, and perform triangular solves with a dense right hand side:

15
gtsam/3rdparty/Eigen/doc/UnalignedArrayAssert.dox vendored
View File

@ -7,8 +7,8 @@ Hello! You are seeing this webpage because your program terminated on an asserti
my_program: path/to/eigen/Eigen/src/Core/DenseStorage.h:44:
Eigen::internal::matrix_array<T, Size, MatrixOptions, Align>::internal::matrix_array()
[with T = double, int Size = 2, int MatrixOptions = 2, bool Align = true]:
Assertion `(reinterpret_cast<size_t>(array) & 0xf) == 0 && "this assertion
is explained here: http://eigen.tuxfamily.org/dox/UnalignedArrayAssert.html
Assertion `(reinterpret_cast<size_t>(array) & (sizemask)) == 0 && "this assertion
is explained here: http://eigen.tuxfamily.org/dox/group__TopicUnalignedArrayAssert.html
**** READ THIS WEB PAGE !!! ****"' failed.
</pre>
@ -46,9 +46,9 @@ then you need to read this separate page: \ref TopicStructHavingEigenMembers "St
Note that here, Eigen::Vector2d is only used as an example, more generally the issue arises for all \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types".
\section c2 Cause 2: STL Containers
\section c2 Cause 2: STL Containers or manual memory allocation
If you use STL Containers such as std::vector, std::map, ..., with Eigen objects, or with classes containing Eigen objects, like this,
If you use STL Containers such as std::vector, std::map, ..., with %Eigen objects, or with classes containing %Eigen objects, like this,
\code
std::vector<Eigen::Matrix2f> my_vector;
@ -60,6 +60,8 @@ then you need to read this separate page: \ref TopicStlContainers "Using STL Con
Note that here, Eigen::Matrix2f is only used as an example, more generally the issue arises for all \ref TopicFixedSizeVectorizable "fixed-size vectorizable Eigen types" and \ref TopicStructHavingEigenMembers "structures having such Eigen objects as member".
The same issue will be exhibited by any classes/functions by-passing operator new to allocate memory, that is, by performing custom memory allocation followed by calls to the placement new operator. This is for instance typically the case of \c std::make_shared or \c std::allocate_shared for which is the solution is to use an \ref aligned_allocator "aligned allocator" as detailed in the \ref TopicStlContainers "solution for STL containers".
\section c3 Cause 3: Passing Eigen objects by value
If some function in your code is getting an Eigen object passed by value, like this,
@ -107,7 +109,10 @@ Two possibilities:
128-bit alignment code and thus preserves ABI compatibility, but completely disables vectorization.</li>
</ul>
For more information, see <a href="http://eigen.tuxfamily.org/index.php?title=FAQ#I_disabled_vectorization.2C_but_I.27m_still_getting_annoyed_about_alignment_issues.21">this FAQ</a>.
If you want to know why defining EIGEN_DONT_VECTORIZE does not by itself disable 128-bit alignment and the assertion, here's the explanation:
It doesn't disable the assertion, because otherwise code that runs fine without vectorization would suddenly crash when enabling vectorization.
It doesn't disable 128bit alignment, because that would mean that vectorized and non-vectorized code are not mutually ABI-compatible. This ABI compatibility is very important, even for people who develop only an in-house application, as for instance one may want to have in the same application a vectorized path and a non-vectorized path.
*/

7
gtsam/3rdparty/Eigen/eigen3.pc.in vendored
View File

@ -1,6 +1,9 @@
prefix=@CMAKE_INSTALL_PREFIX@
exec_prefix=${prefix}
Name: Eigen3
Description: A C++ template library for linear algebra: vectors, matrices, and related algorithms
Requires:
Version: ${EIGEN_VERSION_NUMBER}
Version: @EIGEN_VERSION_NUMBER@
Libs:
Cflags: -I${INCLUDE_INSTALL_DIR}
Cflags: -I${prefix}/@INCLUDE_INSTALL_DIR@

2
gtsam/3rdparty/Eigen/test/CMakeLists.txt vendored
View File

@ -202,7 +202,9 @@ ei_add_test(geo_alignedbox)
ei_add_test(stdvector)
ei_add_test(stdvector_overload)
ei_add_test(stdlist)
ei_add_test(stdlist_overload)
ei_add_test(stddeque)
ei_add_test(stddeque_overload)
ei_add_test(resize)
ei_add_test(sparse_vector)
ei_add_test(sparse_basic)

39
gtsam/3rdparty/Eigen/test/dynalloc.cpp vendored
View File

@ -87,6 +87,32 @@ template<typename T> void check_dynaligned()
delete obj;
}
template<typename T> void check_custom_new_delete()
{
{
T* t = new T;
delete t;
}
{
std::size_t N = internal::random<std::size_t>(1,10);
T* t = new T[N];
delete[] t;
}
#ifdef EIGEN_ALIGN
{
T* t = static_cast<T *>((T::operator new)(sizeof(T)));
(T::operator delete)(t, sizeof(T));
}
{
T* t = static_cast<T *>((T::operator new)(sizeof(T)));
(T::operator delete)(t);
}
#endif
}
void test_dynalloc()
{
// low level dynamic memory allocation
@ -94,7 +120,9 @@ void test_dynalloc()
CALL_SUBTEST(check_aligned_malloc());
CALL_SUBTEST(check_aligned_new());
CALL_SUBTEST(check_aligned_stack_alloc());
// check static allocation, who knows ?
#if EIGEN_ALIGN_STATICALLY
for (int i=0; i<g_repeat*100; ++i)
{
CALL_SUBTEST(check_dynaligned<Vector4f>() );
@ -102,10 +130,13 @@ void test_dynalloc()
CALL_SUBTEST(check_dynaligned<Matrix4f>() );
CALL_SUBTEST(check_dynaligned<Vector4d>() );
CALL_SUBTEST(check_dynaligned<Vector4i>() );
CALL_SUBTEST( check_custom_new_delete<Vector4f>() );
CALL_SUBTEST( check_custom_new_delete<Vector2f>() );
CALL_SUBTEST( check_custom_new_delete<Matrix4f>() );
CALL_SUBTEST( check_custom_new_delete<MatrixXi>() );
}
// check static allocation, who knows ?
#if EIGEN_ALIGN_STATICALLY
{
MyStruct foo0; VERIFY(size_t(foo0.avec.data())%ALIGNMENT==0);
MyClassA fooA; VERIFY(size_t(fooA.avec.data())%ALIGNMENT==0);

37
gtsam/3rdparty/Eigen/test/product.h vendored
View File

@ -136,9 +136,27 @@ template<typename MatrixType> void product(const MatrixType& m)
VERIFY_IS_APPROX(res.col(r).noalias() = square.adjoint() * square.col(r), (square.adjoint() * square.col(r)).eval());
VERIFY_IS_APPROX(res.col(r).noalias() = square * square.col(r), (square * square.col(r)).eval());
// vector at runtime (see bug 1166)
{
RowSquareMatrixType ref(square);
ColSquareMatrixType ref2(square2);
ref = res = square;
VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.col(0).transpose() * square.transpose(), (ref.row(0) = m1.col(0).transpose() * square.transpose()));
VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.block(0,0,rows,1).transpose() * square.transpose(), (ref.row(0) = m1.col(0).transpose() * square.transpose()));
VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.col(0).transpose() * square, (ref.row(0) = m1.col(0).transpose() * square));
VERIFY_IS_APPROX(res.block(0,0,1,rows).noalias() = m1.block(0,0,rows,1).transpose() * square, (ref.row(0) = m1.col(0).transpose() * square));
ref2 = res2 = square2;
VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.row(0) * square2.transpose(), (ref2.row(0) = m1.row(0) * square2.transpose()));
VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.block(0,0,1,cols) * square2.transpose(), (ref2.row(0) = m1.row(0) * square2.transpose()));
VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.row(0) * square2, (ref2.row(0) = m1.row(0) * square2));
VERIFY_IS_APPROX(res2.block(0,0,1,cols).noalias() = m1.block(0,0,1,cols) * square2, (ref2.row(0) = m1.row(0) * square2));
}
// inner product
Scalar x = square2.row(c) * square2.col(c2);
VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum());
{
Scalar x = square2.row(c) * square2.col(c2);
VERIFY_IS_APPROX(x, square2.row(c).transpose().cwiseProduct(square2.col(c2)).sum());
}
// outer product
VERIFY_IS_APPROX(m1.col(c) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
@ -146,5 +164,18 @@ template<typename MatrixType> void product(const MatrixType& m)
VERIFY_IS_APPROX(m1.block(0,c,rows,1) * m1.row(r), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
VERIFY_IS_APPROX(m1.col(c) * m1.block(r,0,1,cols), m1.block(0,c,rows,1) * m1.block(r,0,1,cols));
VERIFY_IS_APPROX(m1.leftCols(1) * m1.row(r), m1.block(0,0,rows,1) * m1.block(r,0,1,cols));
VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols));
VERIFY_IS_APPROX(m1.col(c) * m1.topRows(1), m1.block(0,c,rows,1) * m1.block(0,0,1,cols));
// Aliasing
{
ColVectorType x(cols); x.setRandom();
ColVectorType z(x);
ColVectorType y(cols); y.setZero();
ColSquareMatrixType A(cols,cols); A.setRandom();
// CwiseBinaryOp
VERIFY_IS_APPROX(x = y + A*x, A*z);
x = z;
// CwiseUnaryOp
VERIFY_IS_APPROX(x = Scalar(1.)*(A*x), A*z);
}
}

23
gtsam/3rdparty/Eigen/test/product_large.cpp vendored
View File

@ -9,6 +9,27 @@
#include "product.h"
template<typename T>
void test_aliasing()
{
int rows = internal::random<int>(1,12);
int cols = internal::random<int>(1,12);
typedef Matrix<T,Dynamic,Dynamic> MatrixType;
typedef Matrix<T,Dynamic,1> VectorType;
VectorType x(cols); x.setRandom();
VectorType z(x);
VectorType y(rows); y.setZero();
MatrixType A(rows,cols); A.setRandom();
// CwiseBinaryOp
VERIFY_IS_APPROX(x = y + A*x, A*z);
x = z;
// CwiseUnaryOp
VERIFY_IS_APPROX(x = T(1.)*(A*x), A*z);
x = z;
VERIFY_IS_APPROX(x = y+(-(A*x)), -A*z);
x = z;
}
void test_product_large()
{
for(int i = 0; i < g_repeat; i++) {
@ -17,6 +38,8 @@ void test_product_large()
CALL_SUBTEST_3( product(MatrixXi(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
CALL_SUBTEST_4( product(MatrixXcf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2), internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2))) );
CALL_SUBTEST_5( product(Matrix<float,Dynamic,Dynamic,RowMajor>(internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
CALL_SUBTEST_1( test_aliasing<float>() );
}
#if defined EIGEN_TEST_PART_6

9
gtsam/3rdparty/Eigen/test/product_notemporary.cpp vendored
View File

@ -58,10 +58,19 @@ template<typename MatrixType> void product_notemporary(const MatrixType& m)
r1 = internal::random<Index>(8,rows-r0);
VERIFY_EVALUATION_COUNT( m3 = (m1 * m2.adjoint()), 1);
VERIFY_EVALUATION_COUNT( m3 = (m1 * m2.adjoint()).transpose(), 1);
VERIFY_EVALUATION_COUNT( m3.noalias() = m1 * m2.adjoint(), 0);
VERIFY_EVALUATION_COUNT( m3 = s1 * (m1 * m2.transpose()), 1);
VERIFY_EVALUATION_COUNT( m3 = m3 + s1 * (m1 * m2.transpose()), 1);
VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * (m1 * m2.transpose()), 0);
VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()), 1);
VERIFY_EVALUATION_COUNT( m3 = m3 + (m1 * m2.adjoint()).transpose(), 1);
VERIFY_EVALUATION_COUNT( m3.noalias() = m3 + m1 * m2.transpose(), 1); // 0 in 3.3
VERIFY_EVALUATION_COUNT( m3.noalias() += m3 + m1 * m2.transpose(), 1); // 0 in 3.3
VERIFY_EVALUATION_COUNT( m3.noalias() -= m3 + m1 * m2.transpose(), 1); // 0 in 3.3
VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * m2.adjoint(), 0);
VERIFY_EVALUATION_COUNT( m3.noalias() = s1 * m1 * s2 * (m1*s3+m2*s2).adjoint(), 1);
VERIFY_EVALUATION_COUNT( m3.noalias() = (s1 * m1).adjoint() * s2 * m2, 0);

13
gtsam/3rdparty/Eigen/test/ref.cpp vendored
View File

@ -34,6 +34,18 @@ inline void on_temporary_creation(int) {
// test Ref.h
// Deal with i387 extended precision
#if EIGEN_ARCH_i386 && !(EIGEN_ARCH_x86_64)
#if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_LEAST(4,4)
#pragma GCC optimize ("-ffloat-store")
#else
#undef VERIFY_IS_EQUAL
#define VERIFY_IS_EQUAL(X,Y) VERIFY_IS_APPROX(X,Y)
#endif
#endif
template<typename MatrixType> void ref_matrix(const MatrixType& m)
{
typedef typename MatrixType::Index Index;
@ -71,7 +83,6 @@ template<typename MatrixType> void ref_matrix(const MatrixType& m)
rm2 = m2.block(i,j,brows,bcols);
VERIFY_IS_EQUAL(m1, m2);
ConstRefDynMat rm3 = m1.block(i,j,brows,bcols);
m1.block(i,j,brows,bcols) *= 2;
m2.block(i,j,brows,bcols) *= 2;

5
gtsam/3rdparty/Eigen/test/visitor.cpp vendored
View File

@ -55,6 +55,11 @@ template<typename MatrixType> void matrixVisitor(const MatrixType& p)
VERIFY_IS_APPROX(maxc, eigen_maxc);
VERIFY_IS_APPROX(minc, m.minCoeff());
VERIFY_IS_APPROX(maxc, m.maxCoeff());
eigen_maxc = (m.adjoint()*m).maxCoeff(&eigen_maxrow,&eigen_maxcol);
eigen_maxc = (m.adjoint()*m).eval().maxCoeff(&maxrow,&maxcol);
VERIFY(maxrow == eigen_maxrow);
VERIFY(maxcol == eigen_maxcol);
}
template<typename VectorType> void vectorVisitor(const VectorType& w)

2
gtsam/3rdparty/Eigen/unsupported/Eigen/AlignedVector3 vendored
View File

@ -177,7 +177,7 @@ template<typename _Scalar> class AlignedVector3
}
template<typename Derived>
inline bool isApprox(const MatrixBase<Derived>& other, RealScalar eps=NumTraits<Scalar>::dummy_precision()) const
inline bool isApprox(const MatrixBase<Derived>& other, const RealScalar& eps=NumTraits<Scalar>::dummy_precision()) const
{
return m_coeffs.template head<3>().isApprox(other,eps);
}

9
gtsam/base/Lie.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -295,19 +295,18 @@ private:
};
/**
* Three term approximation of the Baker<EFBFBD>Campbell<EFBFBD>Hausdorff formula
* Three term approximation of the Baker-Campbell-Hausdorff formula
* In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)
* it is not true that Z = X+Y. Instead, Z can be calculated using the BCH
* formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24
* http://en.wikipedia.org/wiki/Baker<EFBFBD>Campbell<EFBFBD>Hausdorff_formula
* http://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula
*/
/// AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
template<class T>
T BCH(const T& X, const T& Y) {
static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
T X_Y = bracket(X, Y);
return X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y,
bracket(X, X_Y));
return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
}
/**

6
gtsam/base/Matrix.cpp
View File

@ -189,7 +189,7 @@ void print(const Matrix& A, const string &s, ostream& stream) {
0, // flags
" ", // coeffSeparator
";\n", // rowSeparator
" \t", // rowPrefix
" \t", // rowPrefix
"", // rowSuffix
"[\n", // matPrefix
"\n ]" // matSuffix
@ -704,11 +704,9 @@ void inplace_QR(Matrix& A){
HCoeffsType hCoeffs(size);
RowVectorType temp(cols);
#ifdef GTSAM_USE_SYSTEM_EIGEN
// System-Eigen is used, and MKL is off
#if !EIGEN_VERSION_AT_LEAST(3,2,5)
Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>(A, hCoeffs, 48, temp.data());
#else
// Patched Eigen is used, and MKL is either on or off
Eigen::internal::householder_qr_inplace_blocked<Matrix, HCoeffsType>::run(A, hCoeffs, 48, temp.data());
#endif

73
gtsam/base/Matrix.h
View File

@ -21,15 +21,17 @@
// \callgraph
#pragma once
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam/config.h> // Configuration from CMake
#include <boost/math/special_functions/fpclassify.hpp>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
/**
@ -532,6 +534,75 @@ GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
/**
* Functor that implements multiplication of a vector b with the inverse of a
* matrix A. The derivatives are inspired by Mike Giles' "An extended collection
* of matrix derivative results for forward and reverse mode algorithmic
* differentiation", at https://people.maths.ox.ac.uk/gilesm/files/NA-08-01.pdf
*/
template <int N>
struct MultiplyWithInverse {
typedef Eigen::Matrix<double, N, 1> VectorN;
typedef Eigen::Matrix<double, N, N> MatrixN;
/// A.inverse() * b, with optional derivatives
VectorN operator()(const MatrixN& A, const VectorN& b,
OptionalJacobian<N, N* N> H1 = boost::none,
OptionalJacobian<N, N> H2 = boost::none) const {
const MatrixN invA = A.inverse();
const VectorN c = invA * b;
// The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
if (H1)
for (size_t j = 0; j < N; j++)
H1->template middleCols<N>(N * j) = -c[j] * invA;
// The derivative in b is easy, as invA*b is just a linear map:
if (H2) *H2 = invA;
return c;
}
};
/**
* Functor that implements multiplication with the inverse of a matrix, itself
* the result of a function f. It turn out we only need the derivatives of the
* operator phi(a): b -> f(a) * b
*/
template <typename T, int N>
struct MultiplyWithInverseFunction {
enum { M = traits<T>::dimension };
typedef Eigen::Matrix<double, N, 1> VectorN;
typedef Eigen::Matrix<double, N, N> MatrixN;
// The function phi should calculate f(a)*b, with derivatives in a and b.
// Naturally, the derivative in b is f(a).
typedef boost::function<VectorN(
const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
Operator;
/// Construct with function as explained above
MultiplyWithInverseFunction(const Operator& phi) : phi_(phi) {}
/// f(a).inverse() * b, with optional derivatives
VectorN operator()(const T& a, const VectorN& b,
OptionalJacobian<N, M> H1 = boost::none,
OptionalJacobian<N, N> H2 = boost::none) const {
MatrixN A;
phi_(a, b, boost::none, A); // get A = f(a) by calling f once
const MatrixN invA = A.inverse();
const VectorN c = invA * b;
if (H1) {
Eigen::Matrix<double, N, M> H;
phi_(a, c, H, boost::none); // get derivative H of forward mapping
*H1 = -invA* H;
}
if (H2) *H2 = invA;
return c;
}
private:
const Operator phi_;
};
} // namespace gtsam
#include <boost/serialization/nvp.hpp>

27
gtsam/base/serializationTestHelpers.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -11,7 +11,7 @@
/**
* @file serializationTestHelpers.h
* @brief
* @brief
* @author Alex Cunningham
* @author Richard Roberts
* @date Feb 7, 2012
@ -30,6 +30,11 @@ const bool verbose = false;
namespace gtsam {
namespace serializationTestHelpers {
// templated default object creation so we only need to declare one friend (if applicable)
template<class T>
T create() {
return T();
}
// Templated round-trip serialization
template<class T>
@ -44,7 +49,7 @@ void roundtrip(const T& input, T& output) {
// This version requires equality operator
template<class T>
bool equality(const T& input = T()) {
T output;
T output = create<T>();
roundtrip<T>(input,output);
return input==output;
}
@ -52,7 +57,7 @@ bool equality(const T& input = T()) {
// This version requires Testable
template<class T>
bool equalsObj(const T& input = T()) {
T output;
T output = create<T>();
roundtrip<T>(input,output);
return assert_equal(input, output);
}
@ -60,7 +65,7 @@ bool equalsObj(const T& input = T()) {
// De-referenced version for pointers, requires equals method
template<class T>
bool equalsDereferenced(const T& input) {
T output;
T output = create<T>();
roundtrip<T>(input,output);
return input->equals(*output);
}
@ -79,7 +84,7 @@ void roundtripXML(const T& input, T& output) {
// This version requires equality operator
template<class T>
bool equalityXML(const T& input = T()) {
T output;
T output = create<T>();
roundtripXML<T>(input,output);
return input==output;
}
@ -87,7 +92,7 @@ bool equalityXML(const T& input = T()) {
// This version requires Testable
template<class T>
bool equalsXML(const T& input = T()) {
T output;
T output = create<T>();
roundtripXML<T>(input,output);
return assert_equal(input, output);
}
@ -95,7 +100,7 @@ bool equalsXML(const T& input = T()) {
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedXML(const T& input = T()) {
T output;
T output = create<T>();
roundtripXML<T>(input,output);
return input->equals(*output);
}
@ -114,7 +119,7 @@ void roundtripBinary(const T& input, T& output) {
// This version requires equality operator
template<class T>
bool equalityBinary(const T& input = T()) {
T output;
T output = create<T>();
roundtripBinary<T>(input,output);
return input==output;
}
@ -122,7 +127,7 @@ bool equalityBinary(const T& input = T()) {
// This version requires Testable
template<class T>
bool equalsBinary(const T& input = T()) {
T output;
T output = create<T>();
roundtripBinary<T>(input,output);
return assert_equal(input, output);
}
@ -130,7 +135,7 @@ bool equalsBinary(const T& input = T()) {
// This version is for pointers, requires equals method
template<class T>
bool equalsDereferencedBinary(const T& input = T()) {
T output;
T output = create<T>();
roundtripBinary<T>(input,output);
return input->equals(*output);
}

4
gtsam/config.h.in
View File

@ -60,4 +60,8 @@
// Option for not throwing the CheiralityException for points that are behind a camera
#cmakedefine GTSAM_THROW_CHEIRALITY_EXCEPTION
// Make sure dependent projects that want it can see deprecated functions
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Publish flag about Eigen typedef
#cmakedefine GTSAM_USE_VECTOR3_POINTS

12
gtsam/geometry/CameraSet.h
View File

@ -110,20 +110,16 @@ public:
std::vector<Z> z(m);
// Allocate derivatives
if (E)
E->resize(ZDim * m, N);
if (Fs)
Fs->resize(m);
if (E) E->resize(ZDim * m, N);
if (Fs) Fs->resize(m);
// Project and fill derivatives
for (size_t i = 0; i < m; i++) {
MatrixZD Fi;
Eigen::Matrix<double, ZDim, N> Ei;
z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
if (Fs)
(*Fs)[i] = Fi;
if (E)
E->block<ZDim, N>(ZDim * i, 0) = Ei;
if (Fs) (*Fs)[i] = Fi;
if (E) E->block<ZDim, N>(ZDim * i, 0) = Ei;
}
return z;

2
gtsam/geometry/EssentialMatrix.cpp
View File

@ -107,7 +107,7 @@ ostream& operator <<(ostream& os, const EssentialMatrix& E) {
Rot3 R = E.rotation();
Unit3 d = E.direction();
os.precision(10);
os << R.xyz().transpose() << " " << d.point3().vector().transpose() << " ";
os << R.xyz().transpose() << " " << d.point3().transpose() << " ";
return os;
}

4
gtsam/geometry/OrientedPlane3.cpp
View File

@ -39,7 +39,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
Unit3 n_rotated = xr.rotation().unrotate(n_, D_rotated_plane, D_rotated_pose);
Vector3 unit_vec = n_rotated.unitVector();
double pred_d = n_.unitVector().dot(xr.translation().vector()) + d_;
double pred_d = n_.unitVector().dot(xr.translation()) + d_;
if (Hr) {
*Hr = zeros(3, 6);
@ -47,7 +47,7 @@ OrientedPlane3 OrientedPlane3::transform(const Pose3& xr, OptionalJacobian<3, 3>
Hr->block<1, 3>(2, 3) = unit_vec;
}
if (Hp) {
Vector2 hpp = n_.basis().transpose() * xr.translation().vector();
Vector2 hpp = n_.basis().transpose() * xr.translation();
*Hp = Z_3x3;
Hp->block<2, 2>(0, 0) = D_rotated_pose;
Hp->block<1, 2>(2, 0) = hpp;

22
gtsam/geometry/PinholeCamera.h
View File

@ -32,6 +32,14 @@ namespace gtsam {
template<typename Calibration>
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
public:
/**
* Some classes template on either PinholeCamera or StereoCamera,
* and this typedef informs those classes what "project" returns.
*/
typedef Point2 Measurement;
private:
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
@ -98,6 +106,20 @@ public:
return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
}
// Create PinholeCamera, with derivatives
static PinholeCamera Create(const Pose3& pose, const Calibration &K,
OptionalJacobian<dimension, 6> H1 = boost::none, //
OptionalJacobian<dimension, DimK> H2 = boost::none) {
typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
if (H1)
*H1 << I_6x6, MatrixK6::Zero();
typedef Eigen::Matrix<double, 6, DimK> Matrix6K;
typedef Eigen::Matrix<double, DimK, DimK> MatrixK;
if (H2)
*H2 << Matrix6K::Zero(), MatrixK::Identity();
return PinholeCamera(pose,K);
}
/// @}
/// @name Advanced Constructors
/// @{

146
gtsam/geometry/Point3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -15,66 +15,54 @@
*/
#include <gtsam/geometry/Point3.h>
#include <cmath>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol
&& fabs(z_ - q.z()) < tol);
#ifndef GTSAM_USE_VECTOR3_POINTS
bool Point3::equals(const Point3 &q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol &&
fabs(z() - q.z()) < tol);
}
/* ************************************************************************* */
void Point3::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
bool Point3::operator==(const Point3& q) const {
return x_ == q.x_ && y_ == q.y_ && z_ == q.z_;
}
/* ************************************************************************* */
Point3 Point3::operator+(const Point3& q) const {
return Point3(x_ + q.x_, y_ + q.y_, z_ + q.z_);
}
/* ************************************************************************* */
Point3 Point3::operator-(const Point3& q) const {
return Point3(x_ - q.x_, y_ - q.y_, z_ - q.z_);
}
/* ************************************************************************* */
Point3 Point3::operator*(double s) const {
return Point3(x_ * s, y_ * s, z_ * s);
}
/* ************************************************************************* */
Point3 Point3::operator/(double s) const {
return Point3(x_ / s, y_ / s, z_ / s);
}
/* ************************************************************************* */
double Point3::distance(const Point3 &p2, OptionalJacobian<1, 3> H1,
double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
double d = (p2 - *this).norm();
if (H1) {
*H1 << x_ - p2.x(), y_ - p2.y(), z_ - p2.z();
*H1 = *H1 *(1. / d);
}
return gtsam::distance(*this,q,H1,H2);
}
if (H2) {
*H2 << -x_ + p2.x(), -y_ + p2.y(), -z_ + p2.z();
*H2 = *H2 *(1. / d);
}
return d;
double Point3::norm(OptionalJacobian<1,3> H) const {
return gtsam::norm(*this, H);
}
Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
return gtsam::normalize(*this, H);
}
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
return gtsam::cross(*this, q, H1, H2);
}
double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
return gtsam::dot(*this, q, H1, H2);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
return os;
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
@ -82,68 +70,66 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3;
return *this - q;
}
#endif
#endif
/* ************************************************************************* */
Point3 Point3::cross(const Point3 &q, OptionalJacobian<3, 3> H_p, OptionalJacobian<3, 3> H_q) const {
if (H_p) {
*H_p << skewSymmetric(-q.vector());
double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
double d = (q - p1).norm();
if (H1) {
*H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z();
*H1 = *H1 *(1. / d);
}
if (H_q) {
*H_q << skewSymmetric(vector());
if (H2) {
*H2 << -p1.x() + q.x(), -p1.y() + q.y(), -p1.z() + q.z();
*H2 = *H2 *(1. / d);
}
return Point3(y_ * q.z_ - z_ * q.y_, z_ * q.x_ - x_ * q.z_,
x_ * q.y_ - y_ * q.x_);
return d;
}
/* ************************************************************************* */
double Point3::dot(const Point3 &q, OptionalJacobian<1, 3> H_p, OptionalJacobian<1, 3> H_q) const {
if (H_p) {
*H_p << q.vector().transpose();
}
if (H_q) {
*H_q << vector().transpose();
}
return (x_ * q.x_ + y_ * q.y_ + z_ * q.z_);
}
/* ************************************************************************* */
double Point3::norm(OptionalJacobian<1,3> H) const {
double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
if (H) {
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r, z_ / r;
*H << p.x() / r, p.y() / r, p.z() / r;
else
*H << 1, 1, 1; // really infinity, why 1 ?
*H << 1, 1, 1; // really infinity, why 1 ?
}
return r;
}
/* ************************************************************************* */
Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
Point3 normalized = *this / norm();
Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) {
Point3 normalized = p / norm(p);
if (H) {
// 3*3 Derivative
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
*H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z();
double xy = p.x() * p.y(), xz = p.x() * p.z(), yz = p.y() * p.z();
*H << y2 + z2, -xy, -xz, /**/ -xy, x2 + z2, -yz, /**/ -xz, -yz, x2 + y2;
*H /= pow(x2 + y2 + z2, 1.5);
}
return normalized;
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point3& p) {
os << '[' << p.x() << ", " << p.y() << ", " << p.z() << "]\';";
return os;
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) {
if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z());
if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z());
return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(),
p.x() * q.y() - p.y() * q.x());
}
double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
if (H1) *H1 << q.x(), q.y(), q.z();
if (H2) *H2 << p.x(), p.y(), p.z();
return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
}
/* ************************************************************************* */

158
gtsam/geometry/Point3.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -21,46 +21,47 @@
#pragma once
#include <gtsam/config.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/Vector.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
#include <cmath>
namespace gtsam {
/**
* A 3D point
#ifdef GTSAM_USE_VECTOR3_POINTS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point3 to Vector3
typedef Vector3 Point3;
#else
/**
* A 3D point is just a Vector3 with some additional methods
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Point3 {
private:
double x_, y_, z_;
class GTSAM_EXPORT Point3 : public Vector3 {
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
/// Default constructor creates a zero-Point3
Point3(): x_(0), y_(0), z_(0) {}
#ifndef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Default constructor no longer initializes, just like Vector3
Point3() {}
#endif
/// Construct from x, y, and z coordinates
Point3(double x, double y, double z): x_(x), y_(y), z_(z) {}
Point3(double x, double y, double z): Vector3(x,y, z) {}
/// @}
/// @name Advanced Constructors
/// @{
/// Construct from 3-element vector
explicit Point3(const Vector3& v) {
x_ = v(0);
y_ = v(1);
z_ = v(2);
}
/// Construct from other vector
template<typename Derived>
inline Point3(const Eigen::MatrixBase<Derived>& v): Vector3(v) {}
/// @}
/// @name Testable
@ -77,46 +78,21 @@ namespace gtsam {
/// @{
/// identity for group operation
inline static Point3 identity() { return Point3();}
/// inverse
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
/// add vector on right
inline Point3 operator +(const Vector3& v) const {
return Point3(x_ + v[0], y_ + v[1], z_ + v[2]);
}
/// add
Point3 operator + (const Point3& q) const;
/// subtract
Point3 operator - (const Point3& q) const;
inline static Point3 identity() { return Point3(0.0, 0.0, 0.0); }
/// @}
/// @name Vector Space
/// @{
///multiply with a scalar
Point3 operator * (double s) const;
///divide by a scalar
Point3 operator / (double s) const;
/** distance between two points */
double distance(const Point3& p2, OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const;
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point3& p2) const {
return (p2 - *this).norm();
}
/** Distance of the point from the origin, with Jacobian */
double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
Point3 normalized(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q, OptionalJacobian<3, 3> H_p = boost::none, //
@ -130,36 +106,27 @@ namespace gtsam {
/// @name Standard Interface
/// @{
/// equality
bool operator ==(const Point3& q) const;
/** return vectorized form (column-wise)*/
Vector3 vector() const { return Vector3(x_,y_,z_); }
/// return as Vector3
const Vector3& vector() const { return *this; }
/// get x
inline double x() const {return x_;}
inline double x() const {return (*this)[0];}
/// get y
inline double y() const {return y_;}
inline double y() const {return (*this)[1];}
/// get z
inline double z() const {return z_;}
/** add two points, add(this,q) is same as this + q */
Point3 add (const Point3 &q,
OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const;
/** subtract two points, sub(this,q) is same as this - q */
Point3 sub (const Point3 &q,
OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
inline double z() const {return (*this)[2];}
/// @}
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point3() { setZero(); } // initializes to zero, in contrast to new behavior
Point3 inverse() const { return -(*this);}
Point3 compose(const Point3& q) const { return (*this)+q;}
Point3 between(const Point3& q) const { return q-(*this);}
@ -167,39 +134,62 @@ namespace gtsam {
Point3 retract(const Vector3& v) const { return compose(Point3(v));}
static Vector3 Logmap(const Point3& p) { return p.vector();}
static Point3 Expmap(const Vector3& v) { return Point3(v);}
/// @}
inline double dist(const Point3& q) const { return (q - *this).norm(); }
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
Point3 add(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
Point3 sub(const Point3& q, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// @}
#endif
private:
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
{
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
ar & BOOST_SERIALIZATION_NVP(z_);
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector3);
}
/// @}
};
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
/// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;}
template<>
struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
#endif
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
/// distance between two points
double distance(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none);
/// Distance of the point from the origin, with Jacobian
double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
/// normalize, with optional Jacobian
Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
/// cross product @return this x q
Point3 cross(const Point3& p, const Point3& q,
OptionalJacobian<3, 3> H_p = boost::none,
OptionalJacobian<3, 3> H_q = boost::none);
/// dot product
double dot(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H_p = boost::none,
OptionalJacobian<1, 3> H_q = boost::none);
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
template <typename A1, typename A2>
struct Range;
@ -209,7 +199,7 @@ struct Range<Point3, Point3> {
double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return p.distance(q, H1, H2);
return distance(p, q, H1, H2);
}
};

58
gtsam/geometry/Pose3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -48,8 +48,7 @@ Pose3 Pose3::inverse() const {
// Experimental - unit tests of derivatives based on it do not check out yet
Matrix6 Pose3::AdjointMap() const {
const Matrix3 R = R_.matrix();
const Vector3 t = t_.vector();
Matrix3 A = skewSymmetric(t) * R;
Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
Matrix6 adj;
adj << R, Z_3x3, A, R;
return adj;
@ -101,12 +100,12 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
void Pose3::print(const string& s) const {
cout << s;
R_.print("R:\n");
t_.print("t: ");
cout << '[' << t_.x() << ", " << t_.y() << ", " << t_.z() << "]\';";
}
/* ************************************************************************* */
bool Pose3::equals(const Pose3& pose, double tol) const {
return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol);
return R_.equals(pose.R_, tol) && traits<Point3>::Equals(t_, pose.t_, tol);
}
/* ************************************************************************* */
@ -115,14 +114,14 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
if (H) *H = ExpmapDerivative(xi);
// get angular velocity omega and translational velocity v from twist xi
Point3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
Vector3 omega(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
Rot3 R = Rot3::Expmap(omega.vector());
Rot3 R = Rot3::Expmap(omega);
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
Point3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Point3 omega_cross_v = omega.cross(v); // points towards axis
Point3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
Vector3 t_parallel = omega * omega.dot(v); // translation parallel to axis
Vector3 omega_cross_v = omega.cross(v); // points towards axis
Vector3 t = (omega_cross_v - R * omega_cross_v + t_parallel) / theta2;
return Pose3(R, t);
} else {
return Pose3(R, v);
@ -132,19 +131,20 @@ Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
/* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
if (H) *H = LogmapDerivative(p);
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = w.norm();
const Vector3 w = Rot3::Logmap(p.rotation());
const Vector3 T = p.translation();
const double t = w.norm();
if (t < 1e-10) {
Vector6 log;
log << w, T;
return log;
} else {
Matrix3 W = skewSymmetric(w / t);
const Matrix3 W = skewSymmetric(w / t);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in T to avoid matrix math
double Tan = tan(0.5 * t);
Vector3 WT = W * T;
Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
const double Tan = tan(0.5 * t);
const Vector3 WT = W * T;
const Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
Vector6 log;
log << w, u;
return log;
@ -178,7 +178,7 @@ Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
H->topLeftCorner<3,3>() = DR;
}
Vector6 xi;
xi << omega, T.translation().vector();
xi << omega, T.translation();
return xi;
#endif
}
@ -264,7 +264,7 @@ const Point3& Pose3::translation(OptionalJacobian<3, 6> H) const {
Matrix4 Pose3::matrix() const {
static const Matrix14 A14 = (Matrix14() << 0.0, 0.0, 0.0, 1.0).finished();
Matrix4 mat;
mat << R_.matrix(), t_.vector(), A14;
mat << R_.matrix(), t_, A14;
return mat;
}
@ -288,7 +288,7 @@ Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
if (Dpoint) {
*Dpoint = R;
}
return Point3(R * p.vector()) + t_;
return R_ * p + t_;
}
/* ************************************************************************* */
@ -297,7 +297,7 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector());
const Point3 q(Rt*(p - t_));
if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z();
(*Dpose) <<
@ -321,7 +321,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
return local.norm();
} else {
Matrix13 D_r_local;
const double r = local.norm(D_r_local);
const double r = norm(local, D_r_local);
if (H1) *H1 = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point;
return r;
@ -361,10 +361,10 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
return boost::none; // we need at least three pairs
// calculate centroids
Vector3 cp = Vector3::Zero(), cq = Vector3::Zero();
Point3 cp(0,0,0), cq(0,0,0);
BOOST_FOREACH(const Point3Pair& pair, pairs) {
cp += pair.first.vector();
cq += pair.second.vector();
cp += pair.first;
cq += pair.second;
}
double f = 1.0 / n;
cp *= f;
@ -373,10 +373,10 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
// Add to form H matrix
Matrix3 H = Z_3x3;
BOOST_FOREACH(const Point3Pair& pair, pairs) {
Vector3 dp = pair.first.vector() - cp;
Vector3 dq = pair.second.vector() - cq;
Point3 dp = pair.first - cp;
Point3 dq = pair.second - cq;
H += dp * dq.transpose();
}
}
// Compute SVD
Matrix U, V;
@ -394,7 +394,9 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
os << pose.rotation() << "\n" << pose.translation() << endl;
os << pose.rotation() << "\n";
const Point3& t = pose.translation();
os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n";
return os;
}

10
gtsam/geometry/Pose3.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -52,8 +52,7 @@ public:
/// @{
/** Default constructor is origin */
Pose3() {
}
Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
/** Copy constructor */
Pose3(const Pose3& pose) :
@ -65,11 +64,6 @@ public:
R_(R), t_(t) {
}
/** Construct from R,t, where t \in vector3 */
Pose3(const Rot3& R, const Vector3& t) :
R_(R), t_(t) {
}
/** Construct from Pose2 */
explicit Pose3(const Pose2& pose2);

4
gtsam/geometry/Rot3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -82,7 +82,7 @@ Unit3 Rot3::operator*(const Unit3& p) const {
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
Point3 q(Rt * p); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1)
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;

49
gtsam/geometry/Rot3.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -93,7 +93,7 @@ namespace gtsam {
* See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
*/
template<typename Derived>
inline Rot3(const Eigen::MatrixBase<Derived>& R) {
inline explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
#ifdef GTSAM_USE_QUATERNIONS
quaternion_=Matrix3(R);
#else
@ -105,7 +105,7 @@ namespace gtsam {
* Constructor from a rotation matrix
* Overload version for Matrix3 to avoid casting in quaternion mode.
*/
inline Rot3(const Matrix3& R) {
inline explicit Rot3(const Matrix3& R) {
#ifdef GTSAM_USE_QUATERNIONS
quaternion_=R;
#else
@ -171,20 +171,6 @@ namespace gtsam {
return Rot3(q);
}
/**
* Convert from axis/angle representation
* @param axis is the rotation axis, unit length
* @param angle rotation angle
* @return incremental rotation
*/
static Rot3 AxisAngle(const Vector3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return SO3::AxisAngle(axis,angle);
#endif
}
/**
* Convert from axis/angle representation
* @param axisw is the rotation axis, unit length
@ -192,7 +178,11 @@ namespace gtsam {
* @return incremental rotation
*/
static Rot3 AxisAngle(const Point3& axis, double angle) {
return AxisAngle(axis.vector(),angle);
#ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis.vector()));
#else
return Rot3(SO3::AxisAngle(axis,angle));
#endif
}
/**
@ -315,7 +305,7 @@ namespace gtsam {
#ifdef GTSAM_USE_QUATERNIONS
return traits<gtsam::Quaternion>::Expmap(v);
#else
return traits<SO3>::Expmap(v);
return Rot3(traits<SO3>::Expmap(v));
#endif
}
@ -352,17 +342,6 @@ namespace gtsam {
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
/// operator* for Vector3
inline Vector3 operator*(const Vector3& v) const {
return rotate(Point3(v)).vector();
}
/// rotate for Vector3
Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
return rotate(Point3(v), H1, H2).vector();
}
/// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const;
@ -370,12 +349,6 @@ namespace gtsam {
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
/// unrotate for Vector3
Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
return unrotate(Point3(v), H1, H2).vector();
}
/// @}
/// @name Group Action on Unit3
/// @{
@ -483,7 +456,9 @@ namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
#ifndef GTSAM_USE_VECTOR3_POINTS
static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); }
#endif
static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); }
static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); }
@ -539,7 +514,7 @@ namespace gtsam {
template<>
struct traits<Rot3> : public internal::LieGroup<Rot3> {};
template<>
struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
}

10
gtsam/geometry/Rot3M.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -36,9 +36,9 @@ Rot3::Rot3() : rot_(I_3x3) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
rot_.col(0) = col1.vector();
rot_.col(1) = col2.vector();
rot_.col(2) = col3.vector();
rot_.col(0) = (Vector3)col1;
rot_.col(1) = (Vector3)col2;
rot_.col(2) = (Vector3)col3;
}
/* ************************************************************************* */
@ -121,7 +121,7 @@ Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
return Point3(rot_ * p.vector());
return rot_ * p;
}
/* ************************************************************************* */

215
gtsam/geometry/SO3.cpp
View File

@ -11,8 +11,10 @@
/**
* @file SO3.cpp
* @brief 3*3 matrix representation o SO(3)
* @brief 3*3 matrix representation of SO(3)
* @author Frank Dellaert
* @author Luca Carlone
* @author Duy Nguyen Ta
* @date December 2014
*/
@ -23,66 +25,109 @@
namespace gtsam {
namespace so3 {
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (nearZero) return;
theta = std::sqrt(theta2); // rotation angle
sin_theta = std::sin(theta);
const double s2 = std::sin(theta / 2.0);
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
: theta2(omega.dot(omega)) {
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
init(nearZeroApprox);
if (!nearZero) {
theta = std::sqrt(theta2);
K = W / theta;
KK = K * K;
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox)
: theta2(angle * angle) {
const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W = K * angle;
init(nearZeroApprox);
if (!nearZero) {
theta = angle;
KK = K * K;
}
}
SO3 ExpmapFunctor::expmap() const {
if (nearZero)
return I_3x3 + W;
else
return I_3x3 + sin_theta * K + one_minus_cos * KK;
}
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero)
dexp_ = I_3x3 - 0.5 * W;
else {
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
}
}
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
if (H1) {
if (nearZero) {
*H1 = 0.5 * skewSymmetric(v);
} else {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Vector3 Kv = K * v;
const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
}
}
if (H2) *H2 = dexp_;
return dexp_ * v;
}
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = dexp_.inverse();
const Vector3 c = invDexp * v;
if (H1) {
Matrix3 D_dexpv_omega;
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
*H1 = -invDexp* D_dexpv_omega;
}
if (H2) *H2 = invDexp;
return c;
}
} // namespace so3
/* ************************************************************************* */
// Near zero, we just have I + skew(omega)
static SO3 FirstOrder(const Vector3& omega) {
Matrix3 R;
R(0, 0) = 1.;
R(1, 0) = omega.z();
R(2, 0) = -omega.y();
R(0, 1) = -omega.z();
R(1, 1) = 1.;
R(2, 1) = omega.x();
R(0, 2) = omega.y();
R(1, 2) = -omega.x();
R(2, 2) = 1.;
return R;
}
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
if (theta*theta > std::numeric_limits<double>::epsilon()) {
using std::cos;
using std::sin;
// get components of axis \omega, where is a unit vector
const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
const double costheta = cos(theta), sintheta = sin(theta), s2 = sin(theta/2.0), c_1 = 2.0*s2*s2;
const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
wz_sintheta = wz * sintheta;
const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
const double C22 = c_1 * wz * wz;
Matrix3 R;
R(0, 0) = costheta + C00;
R(1, 0) = wz_sintheta + C01;
R(2, 0) = -wy_sintheta + C02;
R(0, 1) = -wz_sintheta + C01;
R(1, 1) = costheta + C11;
R(2, 1) = wx_sintheta + C12;
R(0, 2) = wy_sintheta + C02;
R(1, 2) = -wx_sintheta + C12;
R(2, 2) = costheta + C22;
return R;
} else {
return FirstOrder(axis*theta);
}
return so3::ExpmapFunctor(axis, theta).expmap();
}
/// simply convert omega to axis/angle representation
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) *H = ExpmapDerivative(omega);
if (H) {
so3::DexpFunctor impl(omega);
*H = impl.dexp();
return impl.expmap();
} else
return so3::ExpmapFunctor(omega).expmap();
}
double theta2 = omega.dot(omega);
if (theta2 > std::numeric_limits<double>::epsilon()) {
double theta = std::sqrt(theta2);
return AxisAngle(omega / theta, theta);
} else {
return FirstOrder(omega);
}
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
return so3::DexpFunctor(omega).dexp();
}
/* ************************************************************************* */
@ -96,7 +141,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
// Get trace(R)
double tr = R.trace();
const double tr = R.trace();
Vector3 omega;
@ -112,7 +157,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else {
double magnitude;
double tr_3 = tr - 3.0; // always negative
const double tr_3 = tr - 3.0; // always negative
if (tr_3 < -1e-7) {
double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta));
@ -129,58 +174,13 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
}
/* ************************************************************************* */
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
using std::sin;
const double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
const double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
return I_3x3 - 0.5*s1*s1*X + s2*X2;
#else // Luca's version
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
* where Jr = ExpmapDerivative(thetahat);
* This maps a perturbation in the tangent space (omega) to
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = omega^, normalized by normx
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
Matrix3 Y;
Y << 0.0, -wz, +wy,
+wz, 0.0, -wx,
-wy, +wx, 0.0;
const double s2 = sin(theta / 2.0);
const double a = (2.0 * s2 * s2 / theta2);
const double b = (1.0 - sin(theta) / theta) / theta2;
return I_3x3 - a * Y + b * Y * Y;
#endif
}
/* ************************************************************************* */
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
using std::cos;
using std::sin;
double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
return I_3x3 + 0.5*X - s2*X2;
#else // Luca's version
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
@ -188,11 +188,10 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^
return I_3x3 + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
* X;
#endif
const Matrix3 W = skewSymmetric(omega); // element of Lie algebra so(3): W = omega^
return I_3x3 + 0.5 * W +
(1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
W * W;
}
/* ************************************************************************* */

63
gtsam/geometry/SO3.h
View File

@ -13,6 +13,8 @@
* @file SO3.h
* @brief 3*3 matrix representation of SO(3)
* @author Frank Dellaert
* @author Luca Carlone
* @author Duy Nguyen Ta
* @date December 2014
*/
@ -97,15 +99,15 @@ public:
*/
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
/// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& omega);
/**
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
/// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& omega);
/// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& omega);
@ -128,6 +130,61 @@ public:
/// @}
};
// This namespace exposes two functors that allow for saving computation when
// exponential map and its derivatives are needed at the same location in so<3>
// The second functor also implements dedicated methods to apply dexp and/or inv(dexp)
namespace so3 {
/// Functor implementing Exponential map
class ExpmapFunctor {
protected:
const double theta2;
Matrix3 W, K, KK;
bool nearZero;
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
void init(bool nearZeroApprox = false);
public:
/// Constructor with element of Lie algebra so(3)
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
/// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
/// Rodrigues formula
SO3 expmap() const;
};
/// Functor that implements Exponential map *and* its derivatives
class DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
double a, b;
Matrix3 dexp_;
public:
/// Constructor with element of Lie algebra so(3)
DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
// Information Theory, and Lie Groups", Volume 2, 2008.
// expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
// This maps a perturbation v in the tangent space to
// a perturbation on the manifold Expmap(dexp * v) */
const Matrix3& dexp() const { return dexp_; }
/// Multiplies with dexp(), with optional derivatives
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// Multiplies with dexp().inverse(), with optional derivatives
Vector3 applyInvDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
};
} // namespace so3
template<>
struct traits<SO3> : public internal::LieGroup<SO3> {
};

29
gtsam/geometry/Unit3.cpp
View File

@ -35,6 +35,7 @@
#include <boost/random/variate_generator.hpp>
#include <iostream>
#include <limits>
#include <cmath>
using namespace std;
@ -44,9 +45,8 @@ namespace gtsam {
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
// 3*3 Derivative of representation with respect to point is 3*3:
Matrix3 D_p_point;
Point3 normalized = point.normalize(H ? &D_p_point : 0);
Unit3 direction;
direction.p_ = normalized.vector();
direction.p_ = normalize(point, H ? &D_p_point : 0);
if (H)
*H << direction.basis().transpose() * D_p_point;
return direction;
@ -89,31 +89,27 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const {
const Point3 n(p_);
// Get the axis of rotation with the minimum projected length of the point
Point3 axis;
Point3 axis(0, 0, 1);
double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z());
if ((mx <= my) && (mx <= mz)) {
axis = Point3(1.0, 0.0, 0.0);
} else if ((my <= mx) && (my <= mz)) {
axis = Point3(0.0, 1.0, 0.0);
} else if ((mz <= mx) && (mz <= my)) {
axis = Point3(0.0, 0.0, 1.0);
} else {
assert(false);
}
// Choose the direction of the first basis vector b1 in the tangent plane by crossing n with
// the chosen axis.
Matrix33 H_B1_n;
Point3 B1 = n.cross(axis, H ? &H_B1_n : 0);
Point3 B1 = gtsam::cross(n, axis, H ? &H_B1_n : 0);
// Normalize result to get a unit vector: b1 = B1 / |B1|.
Matrix33 H_b1_B1;
Point3 b1 = B1.normalize(H ? &H_b1_B1 : 0);
Point3 b1 = normalize(B1, H ? &H_b1_B1 : 0);
// Get the second basis vector b2, which is orthogonal to n and b1, by crossing them.
// No need to normalize this, p and b1 are orthogonal unit vectors.
Matrix33 H_b2_n, H_b2_b1;
Point3 b2 = n.cross(b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0);
Point3 b2 = gtsam::cross(n, b1, H ? &H_b2_n : 0, H ? &H_b2_b1 : 0);
// Create the basis by stacking b1 and b2.
B_.reset(Matrix32());
@ -175,7 +171,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1,
// Compute the dot product of the Point3s.
Matrix13 H_dot_pn, H_dot_qn;
double d = pn.dot(qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0);
double d = gtsam::dot(pn, qn, H_p ? &H_dot_pn : 0, H_q ? &H_dot_qn : 0);
if (H_p) {
(*H_p) << H_dot_pn * H_pn_p;
@ -208,7 +204,7 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
// 2D error here is projecting q into the tangent plane of this (p).
Matrix62 H_B_p;
Matrix23 Bt = basis(H_p ? &H_B_p : 0).transpose();
Vector2 xi = Bt * qn.vector();
Vector2 xi = Bt * qn;
if (H_p) {
// Derivatives of each basis vector.
@ -216,8 +212,8 @@ Vector2 Unit3::errorVector(const Unit3& q, OptionalJacobian<2, 2> H_p, OptionalJ
const Matrix32& H_b2_p = H_B_p.block<3, 2>(3, 0);
// Derivatives of the two entries of xi wrt the basis vectors.
Matrix13 H_xi1_b1 = qn.vector().transpose();
Matrix13 H_xi2_b2 = qn.vector().transpose();
Matrix13 H_xi1_b1 = qn.transpose();
Matrix13 H_xi2_b2 = qn.transpose();
// Assemble dxi/dp = dxi/dB * dB/dp.
Matrix12 H_xi1_p = H_xi1_b1 * H_b1_p;
@ -252,11 +248,10 @@ Unit3 Unit3::retract(const Vector2& v) const {
// Treat case of very small v differently
if (theta < std::numeric_limits<double>::epsilon()) {
return Unit3(cos(theta) * p_ + xi_hat);
return Unit3(Vector3(std::cos(theta) * p_ + xi_hat));
}
Vector3 exp_p_xi_hat =
cos(theta) * p_ + xi_hat * (sin(theta) / theta);
Vector3 exp_p_xi_hat = std::cos(theta) * p_ + xi_hat * (sin(theta) / theta);
return Unit3(exp_p_xi_hat);
}

2
gtsam/geometry/Unit3.h
View File

@ -65,10 +65,12 @@ public:
p_(1.0, 0.0, 0.0) {
}
#ifndef GTSAM_USE_VECTOR3_POINTS
/// Construct from point
explicit Unit3(const Point3& p) :
p_(p.vector().normalized()) {
}
#endif
/// Construct from a vector3
explicit Unit3(const Vector3& p) :

12
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -30,13 +30,9 @@ using namespace gtsam;
GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
// Camera situated at 0.5 meters high, looking down
static const Pose3 pose1((Matrix3() <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0,0,0.5));
static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
Point3(0, 0, 0.5));
static const CalibratedCamera camera(pose1);
static const Point3 point1(-0.08,-0.08, 0.0);

37
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -34,7 +34,7 @@ typedef PinholeCamera<Cal3_S2> Camera;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Camera camera(pose, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
@ -57,6 +57,21 @@ TEST( PinholeCamera, constructor)
EXPECT(assert_equal( pose, camera.pose()));
}
//******************************************************************************
TEST(PinholeCamera, Create) {
Matrix actualH1, actualH2;
EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2)));
// Check derivative
boost::function<Camera(Pose3,Cal3_S2)> f = //
boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
}
//******************************************************************************
TEST(PinholeCamera, Pose) {
@ -88,16 +103,16 @@ TEST( PinholeCamera, level2)
TEST( PinholeCamera, lookat)
{
// Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0);
Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
Point3 C(10,0,0);
Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
EXPECT(assert_equal( camera.pose(), expected));
EXPECT(assert_equal(camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Point3 C2(30,0,10);
Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
@ -134,7 +149,7 @@ TEST( PinholeCamera, backprojectInfinity)
/* ************************************************************************* */
TEST( PinholeCamera, backproject2)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
@ -150,7 +165,7 @@ TEST( PinholeCamera, backproject2)
/* ************************************************************************* */
TEST( PinholeCamera, backprojectInfinity2)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
@ -165,7 +180,7 @@ TEST( PinholeCamera, backprojectInfinity2)
/* ************************************************************************* */
TEST( PinholeCamera, backprojectInfinity3)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity
Camera camera(Pose3(rot, origin), K);
@ -263,7 +278,7 @@ TEST( PinholeCamera, range0) {
double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result,
1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));

17
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -35,7 +35,7 @@ typedef PinholePose<Cal3_S2> Camera;
static const Cal3_S2::shared_ptr K = boost::make_shared<Cal3_S2>(625, 625, 0, 0, 0);
static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Camera camera(pose, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
@ -74,16 +74,16 @@ TEST(PinholeCamera, Pose) {
TEST( PinholePose, lookat)
{
// Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0);
Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
Point3 C(10,0,0);
Camera camera = Camera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
EXPECT(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Point3 C2(30,0,10);
Camera camera2 = Camera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
@ -120,7 +120,7 @@ TEST( PinholePose, backprojectInfinity)
/* ************************************************************************* */
TEST( PinholePose, backproject2)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
@ -212,8 +212,7 @@ TEST( PinholePose, range0) {
double result = camera.range(point1, D1, D2);
Matrix expectedDcamera = numericalDerivative21(range0, camera, point1);
Matrix expectedDpoint = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
1e-9);
EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9);
EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
}

38
gtsam/geometry/tests/testPoint3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -61,44 +61,46 @@ TEST(Point3, Lie) {
/* ************************************************************************* */
TEST( Point3, arithmetic) {
CHECK(P * 3 == 3 * P);
CHECK(assert_equal(Point3(-1, -5, -6), -Point3(1, 5, 6)));
CHECK(assert_equal(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
CHECK(assert_equal(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
CHECK(assert_equal(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
CHECK(assert_equal(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
CHECK(assert_equal(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
CHECK(assert_equal<Point3>(Point3(-1, -5, -6), -Point3(1, 5, 6)));
CHECK(assert_equal<Point3>(Point3(2, 5, 6), Point3(1, 4, 5) + Point3(1, 1, 1)));
CHECK(assert_equal<Point3>(Point3(0, 3, 4), Point3(1, 4, 5) - Point3(1, 1, 1)));
CHECK(assert_equal<Point3>(Point3(2, 8, 6), Point3(1, 4, 3) * 2));
CHECK(assert_equal<Point3>(Point3(2, 2, 6), 2 * Point3(1, 1, 3)));
CHECK(assert_equal<Point3>(Point3(1, 2, 3), Point3(2, 4, 6) / 2));
}
/* ************************************************************************* */
TEST( Point3, equals) {
CHECK(P.equals(P));
Point3 Q;
CHECK(!P.equals(Q));
CHECK(traits<Point3>::Equals(P,P));
Point3 Q(0,0,0);
CHECK(!traits<Point3>::Equals(P,Q));
}
/* ************************************************************************* */
TEST( Point3, dot) {
Point3 origin, ones(1, 1, 1);
Point3 origin(0,0,0), ones(1, 1, 1);
CHECK(origin.dot(Point3(1, 1, 0)) == 0);
CHECK(ones.dot(Point3(1, 1, 0)) == 2);
}
/* ************************************************************************* */
#ifndef GTSAM_USE_VECTOR3_POINTS
TEST( Point3, stream) {
Point3 p(1, 2, -3);
std::ostringstream os;
os << p;
EXPECT(os.str() == "[1, 2, -3]';");
}
#endif
//*************************************************************************
TEST (Point3, normalize) {
Matrix actualH;
Point3 point(1, -2, 3); // arbitrary point
Point3 expected(point / sqrt(14.0));
EXPECT(assert_equal(expected, point.normalize(actualH), 1e-8));
EXPECT(assert_equal(expected, normalize(point, actualH), 1e-8));
Matrix expectedH = numericalDerivative11<Point3, Point3>(
boost::bind(&Point3::normalize, _1, boost::none), point);
boost::bind(gtsam::normalize, _1, boost::none), point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
@ -111,20 +113,20 @@ TEST (Point3, norm) {
Matrix actualH;
Point3 point(3,4,5); // arbitrary point
double expected = sqrt(50);
EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8);
EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
double testFunc(const Point3& P, const Point3& Q) {
return P.distance(Q);
return distance(P,Q);
}
TEST (Point3, distance) {
Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
Matrix H1, H2;
double d = P.distance(Q, H1, H2);
double d = distance(P, Q, H1, H2);
double expectedDistance = 55.542686;
Matrix numH1 = numericalDerivative21(testFunc, P, Q);
Matrix numH2 = numericalDerivative22(testFunc, P, Q);
@ -137,9 +139,9 @@ TEST (Point3, distance) {
TEST(Point3, cross) {
Matrix aH1, aH2;
boost::function<Point3(const Point3&, const Point3&)> f =
boost::bind(&Point3::cross, _1, _2, boost::none, boost::none);
boost::bind(&gtsam::cross, _1, _2, boost::none, boost::none);
const Point3 omega(0, 1, 0), theta(4, 6, 8);
omega.cross(theta, aH1, aH2);
cross(omega, theta, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
}

16
gtsam/geometry/tests/testPose3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -62,7 +62,7 @@ TEST( Pose3, retract_first_order)
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2));
EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), id.retract(v),1e-2));
v(3)=0.2;v(4)=0.7;v(5)=-2;
EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
}
@ -72,7 +72,7 @@ TEST( Pose3, retract_expmap)
{
Vector v = zero(6); v(0) = 0.3;
Pose3 pose = Pose3::Expmap(v);
EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2));
EXPECT(assert_equal(Pose3(R, Point3(0,0,0)), pose, 1e-2));
EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
}
@ -82,7 +82,7 @@ TEST( Pose3, expmap_a_full)
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3())));
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
}
@ -93,7 +93,7 @@ TEST( Pose3, expmap_a_full2)
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3())));
EXPECT(assert_equal(expmap_default<Pose3>(id, v), Pose3(R, Point3(0,0,0))));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),expmap_default<Pose3>(id, v),1e-5));
}
@ -306,7 +306,7 @@ TEST( Pose3, Dtransform_from1_b)
TEST( Pose3, Dtransform_from1_c)
{
Point3 origin;
Point3 origin(0,0,0);
Pose3 T0(R,origin);
Matrix actualDtransform_from1;
T0.transform_from(P, actualDtransform_from1, boost::none);
@ -388,7 +388,7 @@ TEST( Pose3, transform_to_translate)
/* ************************************************************************* */
TEST( Pose3, transform_to_rotate)
{
Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3());
Pose3 transform(Rot3::Rodrigues(0,0,-1.570796), Point3(0,0,0));
Point3 actual = transform.transform_to(Point3(2,1,10));
Point3 expected(-1,2,10);
EXPECT(assert_equal(expected, actual, 0.001));
@ -406,7 +406,7 @@ TEST( Pose3, transform_to)
/* ************************************************************************* */
TEST( Pose3, transform_from)
{
Point3 actual = T3.transform_from(Point3());
Point3 actual = T3.transform_from(Point3(0,0,0));
Point3 expected = Point3(1.,2.,3.);
EXPECT(assert_equal(expected, actual));
}

156
gtsam/geometry/tests/testRot3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -97,10 +97,10 @@ TEST( Rot3, equals)
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_Rodrigues(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
Matrix3 J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
Matrix3 R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
return Rot3(R);
}
/* ************************************************************************* */
@ -201,7 +201,7 @@ TEST(Rot3, log)
// Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x*PI, y*PI, z*PI).finished();
R = Rot3::Rodrigues(w);
R = Rot3::Rodrigues(w);
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12));
#else
CHECK_OMEGA(x*PI,y*PI,z*PI)
@ -243,129 +243,6 @@ TEST(Rot3, retract_localCoordinates2)
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
}
/* ************************************************************************* */
namespace exmap_derivative {
static const Vector3 w(0.1, 0.27, -0.2);
}
// Left trivialized Derivative of exp(w) wrpt w:
// How does exp(w) change when w changes?
// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector3 testDexpL(const Vector3& dw) {
using exmap_derivative::w;
return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
}
TEST( Rot3, ExpmapDerivative) {
using exmap_derivative::w;
const Matrix actualDexpL = Rot3::ExpmapDerivative(w);
const Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
Vector3::Zero(), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
const Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
}
/* ************************************************************************* */
TEST( Rot3, ExpmapDerivative2)
{
const Vector3 theta(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
}
/* ************************************************************************* */
TEST( Rot3, ExpmapDerivative3)
{
const Vector3 theta(10, 20, 30);
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
}
/* ************************************************************************* */
TEST(Rot3, ExpmapDerivative4) {
// Iserles05an (Lie-group Methods) says:
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
// matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
// where A(t): R -> so(3) is a trajectory in the tangent space of SO(3)
// and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3)
// Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3)
// In GTSAM, we don't work with the skew-symmetric matrices A directly, but with 3-vectors.
// omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t)
// Let's verify the above formula.
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
// We define a function R
auto R = [w](double t) { return Rot3::Expmap(w(t)); };
for (double t = -2.0; t < 2.0; t += 0.3) {
const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
CHECK(assert_equal(expected, actual, 1e-7));
}
}
/* ************************************************************************* */
TEST(Rot3, ExpmapDerivative5) {
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
// Same as above, but define R as mapping local coordinates to neighborhood aroud R0.
const Rot3 R0 = Rot3::Rodrigues(0.1, 0.4, 0.2);
auto R = [R0, w](double t) { return R0.expmap(w(t)); };
for (double t = -2.0; t < 2.0; t += 0.3) {
const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
CHECK(assert_equal(expected, actual, 1e-7));
}
}
/* ************************************************************************* */
TEST( Rot3, jacobianExpmap )
{
const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
&Rot3::Expmap, _1, boost::none), thetahat);
Matrix3 Jactual;
const Rot3 R = Rot3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
TEST( Rot3, LogmapDerivative )
{
const Vector3 thetahat(0.1, 0, 0.1);
const Rot3 R = Rot3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
&Rot3::Logmap, _1, boost::none), R);
const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
TEST( Rot3, JacobianLogmap )
{
const Vector3 thetahat(0.1, 0, 0.1);
const Rot3 R = Rot3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
&Rot3::Logmap, _1, boost::none), R);
Matrix3 Jactual;
Rot3::Logmap(R, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
TEST(Rot3, manifold_expmap)
{
@ -397,14 +274,13 @@ TEST(Rot3, manifold_expmap)
}
/* ************************************************************************* */
class AngularVelocity: public Point3 {
public:
AngularVelocity(const Point3& p) :
Point3(p) {
}
AngularVelocity(double wx, double wy, double wz) :
Point3(wx, wy, wz) {
}
class AngularVelocity : public Vector3 {
public:
template <typename Derived>
inline AngularVelocity(const Eigen::MatrixBase<Derived>& v)
: Vector3(v) {}
AngularVelocity(double wx, double wy, double wz) : Vector3(wx, wy, wz) {}
};
AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
@ -417,10 +293,10 @@ TEST(Rot3, BCH)
// Approximate exmap by BCH formula
AngularVelocity w1(0.2, -0.1, 0.1);
AngularVelocity w2(0.01, 0.02, -0.03);
Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
Rot3 R1 = Rot3::Expmap (w1), R2 = Rot3::Expmap (w2);
Rot3 R3 = R1 * R2;
Vector expected = Rot3::Logmap(R3);
Vector actual = BCH(w1, w2).vector();
Vector actual = BCH(w1, w2);
CHECK(assert_equal(expected, actual,1e-5));
}
@ -666,8 +542,8 @@ TEST(Rot3, quaternion) {
Point3 expected1 = R1*p1;
Point3 expected2 = R2*p2;
Point3 actual1 = Point3(q1*p1.vector());
Point3 actual2 = Point3(q2*p2.vector());
Point3 actual1 = Point3(q1*p1);
Point3 actual2 = Point3(q2*p2);
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(expected2, actual2));

227
gtsam/geometry/tests/testSO3.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -24,16 +24,14 @@ using namespace std;
using namespace gtsam;
//******************************************************************************
TEST(SO3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
BOOST_CONCEPT_ASSERT((IsManifold<SO3 >));
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3 >));
TEST(SO3, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3>));
BOOST_CONCEPT_ASSERT((IsManifold<SO3>));
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3>));
}
//******************************************************************************
TEST(SO3 , Constructor) {
SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
}
TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); }
//******************************************************************************
SO3 id;
@ -42,46 +40,214 @@ SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
//******************************************************************************
TEST(SO3 , Local) {
TEST(SO3, Local) {
Vector3 expected(0, 0, 0.1);
Vector3 actual = traits<SO3>::Local(R1, R2);
EXPECT(assert_equal((Vector)expected,actual));
EXPECT(assert_equal((Vector)expected, actual));
}
//******************************************************************************
TEST(SO3 , Retract) {
TEST(SO3, Retract) {
Vector3 v(0, 0, 0.1);
SO3 actual = traits<SO3>::Retract(R1, v);
EXPECT(actual.isApprox(R2));
}
//******************************************************************************
TEST(SO3 , Invariants) {
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,R1));
EXPECT(check_group_invariants(R2,id));
EXPECT(check_group_invariants(R2,R1));
TEST(SO3, Invariants) {
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, R1));
EXPECT(check_group_invariants(R2, id));
EXPECT(check_group_invariants(R2, R1));
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,R1));
EXPECT(check_manifold_invariants(R2,id));
EXPECT(check_manifold_invariants(R2,R1));
EXPECT(check_manifold_invariants(id, id));
EXPECT(check_manifold_invariants(id, R1));
EXPECT(check_manifold_invariants(R2, id));
EXPECT(check_manifold_invariants(R2, R1));
}
//******************************************************************************
TEST(SO3 , LieGroupDerivatives) {
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,R2);
CHECK_LIE_GROUP_DERIVATIVES(R2,id);
CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
TEST(SO3, LieGroupDerivatives) {
CHECK_LIE_GROUP_DERIVATIVES(id, id);
CHECK_LIE_GROUP_DERIVATIVES(id, R2);
CHECK_LIE_GROUP_DERIVATIVES(R2, id);
CHECK_LIE_GROUP_DERIVATIVES(R2, R1);
}
//******************************************************************************
TEST(SO3 , ChartDerivatives) {
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,R2);
CHECK_CHART_DERIVATIVES(R2,id);
CHECK_CHART_DERIVATIVES(R2,R1);
TEST(SO3, ChartDerivatives) {
CHECK_CHART_DERIVATIVES(id, id);
CHECK_CHART_DERIVATIVES(id, R2);
CHECK_CHART_DERIVATIVES(R2, id);
CHECK_CHART_DERIVATIVES(R2, R1);
}
/* ************************************************************************* */
namespace exmap_derivative {
static const Vector3 w(0.1, 0.27, -0.2);
}
// Left trivialized Derivative of exp(w) wrpt w:
// How does exp(w) change when w changes?
// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector3 testDexpL(const Vector3& dw) {
using exmap_derivative::w;
return SO3::Logmap(SO3::Expmap(-w) * SO3::Expmap(w + dw));
}
TEST(SO3, ExpmapDerivative) {
using exmap_derivative::w;
const Matrix actualDexpL = SO3::ExpmapDerivative(w);
const Matrix expectedDexpL =
numericalDerivative11<Vector3, Vector3>(testDexpL, Vector3::Zero(), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-7));
const Matrix actualDexpInvL = SO3::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7));
}
/* ************************************************************************* */
TEST(SO3, ExpmapDerivative2) {
const Vector3 theta(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
SO3::ExpmapDerivative(-theta)));
}
/* ************************************************************************* */
TEST(SO3, ExpmapDerivative3) {
const Vector3 theta(10, 20, 30);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), theta);
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
SO3::ExpmapDerivative(-theta)));
}
/* ************************************************************************* */
TEST(SO3, ExpmapDerivative4) {
// Iserles05an (Lie-group Methods) says:
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
// matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
// where A(t): R -> so(3) is a trajectory in the tangent space of SO(3)
// and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3)
// Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3)
// In GTSAM, we don't work with the skew-symmetric matrices A directly, but
// with 3-vectors.
// omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t)
// Let's verify the above formula.
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
// We define a function R
auto R = [w](double t) { return SO3::Expmap(w(t)); };
for (double t = -2.0; t < 2.0; t += 0.3) {
const Matrix expected = numericalDerivative11<SO3, double>(R, t);
const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t);
CHECK(assert_equal(expected, actual, 1e-7));
}
}
/* ************************************************************************* */
TEST(SO3, ExpmapDerivative5) {
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
// Now define R as mapping local coordinates to neighborhood around R0.
const SO3 R0 = SO3::Expmap(Vector3(0.1, 0.4, 0.2));
auto R = [R0, w](double t) { return R0.expmap(w(t)); };
for (double t = -2.0; t < 2.0; t += 0.3) {
const Matrix expected = numericalDerivative11<SO3, double>(R, t);
const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t);
CHECK(assert_equal(expected, actual, 1e-7));
}
}
/* ************************************************************************* */
TEST(SO3, ExpmapDerivative6) {
const Vector3 thetahat(0.1, 0, 0.1);
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
boost::bind(&SO3::Expmap, _1, boost::none), thetahat);
Matrix3 Jactual;
SO3::Expmap(thetahat, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
TEST(SO3, LogmapDerivative) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
boost::bind(&SO3::Logmap, _1, boost::none), R);
const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
TEST(SO3, JacobianLogmap) {
const Vector3 thetahat(0.1, 0, 0.1);
const SO3 R = SO3::Expmap(thetahat); // some rotation
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
boost::bind(&SO3::Logmap, _1, boost::none), R);
Matrix3 Jactual;
SO3::Logmap(R, Jactual);
EXPECT(assert_equal(Jexpected, Jactual));
}
/* ************************************************************************* */
TEST(SO3, ApplyDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
};
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
so3::DexpFunctor local(omega, nearZeroApprox);
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
EXPECT(assert_equal(Vector3(local.dexp() * v),
local.applyDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(local.dexp(), aH2));
}
}
}
}
/* ************************************************************************* */
TEST(SO3, ApplyInvDexp) {
Matrix aH1, aH2;
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
};
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
so3::DexpFunctor local(omega, nearZeroApprox);
Matrix invDexp = local.dexp().inverse();
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
EXPECT(assert_equal(Vector3(invDexp * v),
local.applyInvDexp(v, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
EXPECT(assert_equal(invDexp, aH2));
}
}
}
}
//******************************************************************************
@ -90,4 +256,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

22
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -28,13 +28,9 @@ using namespace gtsam;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1((Matrix3() <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0,0,0.5));
static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
Point3(0, 0, 0.5));
static const SimpleCamera camera(pose1, K);
static const Point3 point1(-0.08,-0.08, 0.0);
@ -67,16 +63,16 @@ TEST( SimpleCamera, level2)
TEST( SimpleCamera, lookat)
{
// Create a level camera, looking in Y-direction
Point3 C(10.0,0.0,0.0);
SimpleCamera camera = SimpleCamera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
Point3 C(10,0,0);
SimpleCamera camera = SimpleCamera::Lookat(C, Point3(0,0,0), Point3(0,0,1));
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
CHECK(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Point3 C2(30,0,10);
SimpleCamera camera2 = SimpleCamera::Lookat(C2, Point3(0,0,0), Point3(0,0,1));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
@ -104,7 +100,7 @@ TEST( SimpleCamera, backproject)
/* ************************************************************************* */
TEST( SimpleCamera, backproject2)
{
Point3 origin;
Point3 origin(0,0,0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
SimpleCamera camera(Pose3(rot, origin), K);

8
gtsam/geometry/tests/testStereoCamera.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* 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)
@ -46,7 +46,7 @@ TEST( StereoCamera, project)
// point X Y Z in meters
Point3 p(0, 0, 5);
StereoPoint2 result = stereoCam.project(p);
CHECK(assert_equal(StereoPoint2(320,320-150,240),result));
CHECK(assert_equal(StereoPoint2(320, 320 - 150, 240), result));
// point X Y Z in meters
Point3 p2(1, 1, 5);
@ -74,11 +74,11 @@ TEST( StereoCamera, project)
/* ************************************************************************* */
static Pose3 camPose((Matrix)(Matrix(3,3) <<
static Pose3 camPose(Rot3((Matrix3() <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
).finished()),
Point3(0,0,6.25));
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));

2
gtsam/geometry/tests/testUnit3.cpp
View File

@ -377,7 +377,7 @@ TEST(Unit3, retract_expmap) {
TEST(Unit3, Random) {
boost::mt19937 rng(42);
// Check that means are all zero at least
Point3 expectedMean, actualMean;
Point3 expectedMean(0,0,0), actualMean(0,0,0);
for (size_t i = 0; i < 100; i++)
actualMean = actualMean + Unit3::Random(rng).point3();
actualMean = actualMean / 100;

Some files were not shown because too many files have changed in this diff Show More