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

Merge remote-tracking branch 'origin/develop' into feature/sam_sfm_directories 

# Conflicts:
#	gtsam/CMakeLists.txt
release/4.3a0
cbeall3 2015-06-08 16:18:52 -04:00
parent 8d272f4294 05d7117bea
commit 42d85b6458
803 changed files with 34783 additions and 16383 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3346
.cproject
View File

File diff suppressed because it is too large Load Diff

3
.gitignore vendored
View File

@ -1,7 +1,8 @@
/build*
/doc*
*.pyc
*.DS_Store
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
/examples/Data/pose2example-rewritten.txt
/examples/Data/pose3example-rewritten.txt
*.txt.user
*.txt.user.6d59f0c

1
.settings/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/org.eclipse.cdt.codan.core.prefs

6
.settings/org.eclipse.cdt.core.prefs Normal file
View File

@ -0,0 +1,6 @@
eclipse.preferences.version=1
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/delimiter=\:
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/operation=append
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/value=$PATH\:/opt/local/bin
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true

56
CMakeLists.txt
View File

@ -59,7 +59,7 @@ option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON)
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)
@ -192,37 +192,40 @@ endif()
###############################################################################
# Option for using system Eigen or GTSAM-bundled Eigen
### Disabled until our patches are included in Eigen ###
### These patches only affect usage of MKL. If you want to enable MKL, you *must*
### use our patched version of Eigen
### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization)
# option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
set(GTSAM_USE_SYSTEM_EIGEN OFF)
option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
# Switch for using system Eigen or GTSAM-bundled Eigen
if(GTSAM_USE_SYSTEM_EIGEN)
# Use generic Eigen include paths e.g. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "")
find_package(Eigen3 REQUIRED)
include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
else()
# Use bundled Eigen include paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/")
# Use generic Eigen include paths e.g. <Eigen/Core>
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")
endif()
else()
# Use bundled Eigen include path.
# Clear any variables set by FindEigen3
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
endif()
# Add the bundled version of eigen to the include path so that it can still be included
# with #include <Eigen/Core>
include_directories(BEFORE "gtsam/3rdparty/Eigen/")
# set full path to be used by external projects
# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/")
endif()
# Write Eigen include file with the paths for either the system Eigen or the GTSAM-bundled Eigen
configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eigen_includes.h)
# Install the configuration file for Eigen
install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty)
###############################################################################
# Global compile options
@ -269,18 +272,18 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR})
# paths so that the compiler uses GTSAM headers in our source directory instead
# of any previously installed GTSAM headers.
include_directories(BEFORE
gtsam/3rdparty/UFconfig
gtsam/3rdparty/UFconfig
gtsam/3rdparty/CCOLAMD/Include
gtsam/3rdparty/metis-5.1.0/include
gtsam/3rdparty/metis-5.1.0/libmetis
gtsam/3rdparty/metis-5.1.0/GKlib
gtsam/3rdparty/metis/include
gtsam/3rdparty/metis/libmetis
gtsam/3rdparty/metis/GKlib
${PROJECT_SOURCE_DIR}
${PROJECT_BINARY_DIR} # So we can include generated config header files
CppUnitLite)
if(MSVC)
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
endif()
# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
@ -331,6 +334,7 @@ endif(GTSAM_BUILD_UNSTABLE)
GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
# Check for doxygen availability - optional dependency
find_package(Doxygen)
@ -389,6 +393,11 @@ if(NOT MSVC AND NOT XCODE_VERSION)
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}")
message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}")
endif()
if(GTSAM_USE_SYSTEM_EIGEN)
message(STATUS " Use System Eigen : Yes")
else()
message(STATUS " Use System Eigen : No")
endif()
if(GTSAM_USE_TBB)
message(STATUS " Use Intel TBB : Yes")
elseif(TBB_FOUND)
@ -424,6 +433,7 @@ message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
message(STATUS "MATLAB toolbox flags ")

6
CppUnitLite/Test.h
View File

@ -80,6 +80,12 @@ protected:
testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_)
/**
* Use this to disable unwanted tests without commenting them out.
*/
#define TEST_DISABLED(testGroup, testName)\
void testGroup##testName##Test(TestResult& result_, const std::string& name_)
/*
* Convention for tests:
* - "EXPECT" is a test that will not end execution of the series of tests

206
GTSAM-Concepts.md Normal file
View File

@ -0,0 +1,206 @@
GTSAM Concepts
==============
As discussed in [Generic Programming Techniques](http://www.boost.org/community/generic_programming.html), concepts define
* associated types
* valid expressions, like functions and values
* invariants
* complexity guarantees
Below we discuss the most important concepts use in GTSAM, and after that we discuss how they are implemented/used/enforced.
Manifold
--------
To optimize over continuous types, we assume they are manifolds. This is central to GTSAM and hence discussed in some more detail below.
[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:
* 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.
* 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>`.
* `ManifoldType`, a pointer back to the type.
* `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following:
* `gtsam::traits::manifold_tag` -- Everything in this list is expected
* `gtsam::traits::group_tag` -- The functions defined under **Groups** below.
* `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.
* `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
* `Retract(p, Local(p,q)) == q`
* `Local(p, Retract(p, v)) == v`
Group
-----
A [group]("http://en.wikipedia.org/wiki/Group_(mathematics)"") should be well known from grade school :-), and provides a type with a composition operation that is closed, associative, has an identity element, and an inverse for each element. The following should be added to the traits class for a group:
* valid expressions:
* `r = traits<T>::Compose(p,q)`, where *p*, *q*, and *r* are elements of the manifold.
* `q = traits<T>::Inverse(p)`, where *p* and*q* are elements of the manifold.
* `r = traits<T>::Between(p,q)`, where *p*, *q*, and *r* are elements of the manifold.
* static members:
* `traits<T>::Identity`, a static const member that represents the group's identity element.
* invariants:
* `Compose(p,Inverse(p)) == Identity`
* `Compose(p,Between(p,q)) == q`
* `Between(p,q) == Compose(Inverse(p),q)`
The `gtsam::group::traits` namespace defines the following:
* values:
* `traits<T>::Identity` -- The identity element for this group stored as a static const.
* `traits<T>::group_flavor` -- the flavor of this group's `compose()` operator, either:
* `gtsam::traits::group_multiplicative_tag` for multiplicative operator syntax ,or
* `gtsam::traits::group_additive_tag` for additive operator syntax.
We do *not* at this time support more than one composition operator per type. Although mathematically possible, it is hardly ever needed, and the machinery to support it would be burdensome and counter-intuitive.
Also, a type should provide either multiplication or addition operators depending on the flavor of the operation. To distinguish between the two, we will use a tag (see below).
Lie Group
---------
A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should implements both MANIFOLD and GROUP concepts.
However, we now also need to be able to evaluate the derivatives of compose and inverse.
Hence, we have the following extra valid static functions defined in the struct `gtsam::traits<T>`:
* `r = traits<T>::Compose(p,q,Hq,Hp)`
* `q = traits<T>::Inverse(p,Hp)`
* `r = traits<T>::Between(p,q,Hq,H2p)`
where above the *H* arguments stand for optional Jacobian arguments.
That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor).
In addition, a Lie group has a Lie algebra, which affords two extra valid expressions:
* `v = traits<T>::Logmap(p,Hp)`, the log map, with optional Jacobian
* `p = traits<T>::Expmap(v,Hv)`, the exponential map, with optional Jacobian
Note that in the Lie group case, the usual valid expressions for Retract and Local can be generated automatically, e.g.
```
T Retract(p,v,Hp,Hv) {
T q = Expmap(v,Hqv);
T r = Compose(p,q,Hrp,Hrq);
Hv = Hrq * Hqv; // chain rule
return r;
}
```
For Lie groups, the `exponential map` above is the most obvious mapping: it
associates straight lines in the tangent space with geodesics on the manifold
(and it's inverse, the log map). However, there are two cases in which we deviate from this:
However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`)
Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method.
Vector Space
------------
While vector spaces are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. I.e.where
* `Identity == 0`
* `Inverse(p) == -p`
* `Compose(p,q) == p+q`
* `Between(p,q) == q-p`
* `Local(q) == p-q`
* `Retract(v) == p+v`
This considerably simplifies certain operations. A `VectorSpace` superclass is available to implement the traits. Types that are vector space models include `Matrix`, `Vector`, any fixed or dynamic Eigen Matrix, `Point2`, and `Point3`.
Testable
--------
Unit tests heavily depend on the following two functions being defined for all types that need to be tested:
* valid expressions:
* `Print(p,s)` where s is an optional string
* `Equals(p,q,tol)` where tol is an optional (double) tolerance
Implementation
==============
GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the
TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts.
`gtsam::traits` is our way to associate these concepts with types,
and we also define a limited number of `gtsam::tags` to select the correct implementation
of certain functions at compile time (tag dispatching).
Traits
------
However, a base class is not a good way to implement/check the other concepts, as we would like these
to apply equally well to types that are outside GTSAM control, e.g., `Eigen::VectorXd`. This is where
[traits](http://www.boost.org/doc/libs/1_57_0/libs/type_traits/doc/html/boost_typetraits/background.html) come in.
We use Eigen-style or STL-style traits, that define *many* properties at once.
Note that not everything that makes a concept is defined by traits. Valid expressions such as traits<T>::Compose are
defined simply as static functions within the traits class.
Finally, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types,
rather than having to use traits internally.
Concept Checks
--------------
Boost provides a nice way to check whether a given type satisfies a concept. For example, the following
BOOST_CONCEPT_ASSERT(IsVectorSpace<Point2>)
asserts that Point2 indeed is a model for the VectorSpace concept.
Future Concepts
===============
Group Action
------------
Group actions are concepts in and of themselves that can be concept checked (see below).
In particular, a group can *act* on another space.
For example, the [cyclic group of order 6](http://en.wikipedia.org/wiki/Cyclic_group) can rotate 2D vectors around the origin:
q = R(i)*p
where R(i) = R(60)^i, where R(60) rotates by 60 degrees
Hence, we formalize by the following extension of the concept:
* valid expressions:
* `q = traits<T>::Act(g,p)`, for some instance, *p*, of a space *S*, that can be acted upon by the group element *g* to produce *q* in *S*.
* `q = traits<T>::Act(g,p,Hp)`, if the space acted upon is a continuous differentiable manifold. *
In the latter case, if *S* is an n-dimensional manifold, *Hp* is an output argument that should be
filled with the *nxn* Jacobian matrix of the action with respect to a change in *p*. It typically depends
on the group element *g*, but in most common example will *not* depend on the value of *p*. For example, in
the cyclic group example above, we simply have
Hp = R(i)
Note there is no derivative of the action with respect to a change in g. That will only be defined
for Lie groups, which we introduce now.
Lie Group Action
----------------
When a Lie group acts on a space, we have two derivatives to care about:
* `gtasm::manifold::traits<T>::act(g,p,Hg,Hp)`, if the space acted upon is a continuous differentiable manifold.
An example is a *similarity transform* in 3D, which can act on 3D space, like
q = s*R*p + t
Note that again the derivative in *p*, *Hp* is simply *s R*, which depends on *g* but not on *p*.
The derivative in *g*, *Hg*, is in general more complex.
For now, we won't care about Lie groups acting on non-manifolds.

96
README.md
View File

@ -1,47 +1,51 @@
README - Georgia Tech Smoothing and Mapping library
===================================================
README - Georgia Tech Smoothing and Mapping library
===================================================
What is GTSAM?
--------------
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
Quickstart
----------
In the root library folder execute:
```
#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
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)
Additional Information
----------------------
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
What is GTSAM?
--------------
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
Quickstart
----------
In the root library folder execute:
```
#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
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)
Additional Information
----------------------
See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM.
GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

55
THANKS
View File

@ -1,20 +1,43 @@
GTSAM was made possible by the efforts of many collaborators at Georgia Tech
GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech:
Doru Balcan
Chris Beall
Alex Cunningham
Alireza Fathi
Eohan George
Viorela Ila
Yong-Dian Jian
Michael Kaess
Kai Ni
Carlos Nieto
Duy-Nguyen
Manohar Paluri
Christian Potthast
Richard Roberts
Grant Schindler
* Sungtae An
* Doru Balcan, Bank of America
* Chris Beall
* Luca Carlone
* Alex Cunningham, U Michigan
* Jing Dong
* Alireza Fathi, Stanford
* Eohan George
* Alex Hagiopol
* Viorela Ila, Czeck Republic
* Vadim Indelman, the Technion
* David Jensen, GTRI
* Yong-Dian Jian, Baidu
* Michael Kaess, Carnegie Mellon
* Zhaoyang Lv
* Andrew Melim, Oculus Rift
* Kai Ni, Baidu
* Carlos Nieto
* Duy-Nguyen Ta
* Manohar Paluri, Facebook
* Christian Potthast, USC
* Richard Roberts, Google X
* Grant Schindler, Consultant
* Natesh Srinivasan
* Alex Trevor
* Stephen Williams, BossaNova
at ETH, Zurich
* Paul Furgale
* Mike Bosse
* Hannes Sommer
* Thomas Schneider
at Uni Zurich:
* Christian Forster
Many thanks for your hard work!!!!
Frank Dellaert

41
USAGE → USAGE.md
View File

@ -1,6 +1,5 @@
USAGE - Georgia Tech Smoothing and Mapping library
---------------------------------------------------
===================================
What is this file?
This file explains how to make use of the library for common SLAM tasks,
@ -34,18 +33,12 @@ The GTSAM library has three primary components necessary for the construction
of factor graph representation and optimization which users will need to
adapt to their particular problem.
FactorGraph:
A factor graph contains a set of variables to solve for (i.e., robot poses,
landmark poses, etc.) and a set of constraints between these variables, which
make up factors.
Values:
Values is a single object containing labeled values for all of the
variables. Currently, all variables are labeled with strings, but the type
or organization of the variables can change
Factors:
A nonlinear factor expresses a constraint between variables, which in the
SLAM example, is a measurement such as a visual reading on a landmark or
odometry.
* FactorGraph:
A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors.
* Values:
Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change
* Factors:
A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry.
The library is organized according to the following directory structure:
@ -59,23 +52,3 @@ The library is organized according to the following directory structure:
VSLAM Example
---------------------------------------------------
The visual slam example shows a full implementation of a slam system. The example contains
derived versions of NonlinearFactor, NonlinearFactorGraph, in classes visualSLAM::ProjectionFactor,
visualSLAM::Graph, respectively. The values for the system are stored in the generic
Values structure. For definitions and interface, see gtsam/slam/visualSLAM.h.
The clearest example of the use of the graph to find a solution is in
testVSLAM. The basic process for using graphs is as follows (and can be seen in
the test):
- Create a NonlinearFactorGraph object (visualSLAM::Graph)
- Add factors to the graph (note the use of Boost.shared_ptr here) (visualSLAM::ProjectionFactor)
- Create an initial configuration (Values)
- Create an elimination ordering of variables (this must include all variables)
- Create and initialize a NonlinearOptimizer object (Note that this is a generic
algorithm that does not need to be derived for a particular problem)
- Call optimization functions with the optimizer to optimize the graph
- Extract an updated values from the optimizer

1
cmake/CMakeLists.txt
View File

@ -19,7 +19,6 @@ install(FILES
GtsamMatlabWrap.cmake
GtsamPythonWrap.cmake
GtsamTesting.cmake
GtsamTestingObsolete.cmake
README.html
DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")

22
cmake/GtsamBuildTypes.cmake
View File

@ -15,8 +15,8 @@ option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build typ
# Add debugging flags but only on the first pass
if(NOT FIRST_PASS_DONE)
if(MSVC)
set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
@ -34,8 +34,8 @@ if(NOT FIRST_PASS_DONE)
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING)
else()
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_C_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
@ -53,11 +53,12 @@ if(NOT FIRST_PASS_DONE)
endif()
endif()
# Clang on Mac uses a template depth that is less than standard and is too small
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
endif()
# Clang uses a template depth that is less than standard and is too small
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
# Apple Clang before 5.0 does not support -ftemplate-depth.
if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0"))
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
endif()
endif()
# Set up build type library postfixes
@ -97,7 +98,8 @@ if( NOT cmake_build_type_tolower STREQUAL ""
AND NOT cmake_build_type_tolower STREQUAL "release"
AND NOT cmake_build_type_tolower STREQUAL "timing"
AND NOT cmake_build_type_tolower STREQUAL "profiling"
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo")
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo"
AND NOT cmake_build_type_tolower STREQUAL "minsizerel")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).")
endif()

13
cmake/GtsamMatlabWrap.cmake
View File

@ -270,7 +270,7 @@ function(install_wrapped_library_internal interfaceHeader)
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if("${build_type_upper}" STREQUAL "RELEASE")
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
@ -367,13 +367,18 @@ endfunction()
# should be installed to all build type toolboxes
function(install_matlab_scripts source_directory patterns)
set(patterns_args "")
set(exclude_patterns "")
if(NOT GTSAM_WRAP_SERIALIZATION)
set(exclude_patterns "testSerialization.m")
endif()
foreach(pattern ${patterns})
list(APPEND patterns_args PATTERN "${pattern}")
endforeach()
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if("${build_type_upper}" STREQUAL "RELEASE")
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
@ -381,10 +386,10 @@ function(install_matlab_scripts source_directory patterns)
# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE)
install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endforeach()
else()
install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE)
install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endif()
endfunction()

7
cmake/GtsamTesting.cmake
View File

@ -109,9 +109,8 @@ add_custom_target(examples)
# Add timing target
add_custom_target(timing)
# Include obsolete macros - will be removed in the near future
include(GtsamTestingObsolete)
# Add target to build tests without running
add_custom_target(all.tests)
# Implementations of this file's macros:
@ -165,6 +164,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_test(NAME ${script_name} COMMAND ${script_name})
add_dependencies(check.${groupName} ${script_name})
add_dependencies(check ${script_name})
add_dependencies(all.tests ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
endif()
@ -195,6 +195,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_test(NAME ${target_name} COMMAND ${target_name})
add_dependencies(check.${groupName} ${target_name})
add_dependencies(check ${target_name})
add_dependencies(all.tests ${script_name})
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")

0
cmake/GtsamTestingObsolete.cmake → cmake/obsolete/GtsamTestingObsolete.cmake
View File

1
doc/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/html/

226
doc/Mathematica/Quaternion-Logmap.nb Normal file
View File

@ -0,0 +1,226 @@
(* Content-type: application/vnd.wolfram.mathematica *)
(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)
(* CreatedBy='Mathematica 10.0' *)
(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[ 158, 7]
NotebookDataLength[ 6004, 217]
NotebookOptionsPosition[ 5104, 179]
NotebookOutlinePosition[ 5456, 195]
CellTagsIndexPosition[ 5413, 192]
WindowFrame->Normal*)
(* Beginning of Notebook Content *)
Notebook[{
Cell["\<\
In Quaternion.h we have Logmap, but we have to be careful when qw approaches \
-1 (from above) or 1 (from below). The Taylor expansions below are the basis \
for the code.\
\>", "Text",
CellChangeTimes->{{3.632651837171029*^9, 3.6326518973274307`*^9}}],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"angle", "=",
RowBox[{"2",
RowBox[{"ArcCos", "[", "qw", "]"}]}]}]], "Input",
CellChangeTimes->{{3.6326509558588057`*^9, 3.632650976842943*^9}}],
Cell[BoxData[
RowBox[{"2", " ",
RowBox[{"ArcCos", "[", "qw", "]"}]}]], "Output",
CellChangeTimes->{{3.6326509669341784`*^9, 3.6326509795921097`*^9}}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"s", "=",
RowBox[{"Sqrt", "[",
RowBox[{"1", "-",
RowBox[{"qw", "*", "qw"}]}], "]"}]}]], "Input",
CellChangeTimes->{{3.632650983796185*^9, 3.632650994132272*^9}}],
Cell[BoxData[
SqrtBox[
RowBox[{"1", "-",
SuperscriptBox["qw", "2"]}]]], "Output",
CellChangeTimes->{3.63265099440246*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"factor", " ", "=", " ",
RowBox[{"angle", "/", "s"}]}]], "Input",
CellChangeTimes->{{3.632650999925654*^9, 3.632651001339293*^9}, {
3.632651070297429*^9, 3.632651071527272*^9}}],
Cell[BoxData[
FractionBox[
RowBox[{"2", " ",
RowBox[{"ArcCos", "[", "qw", "]"}]}],
SqrtBox[
RowBox[{"1", "-",
SuperscriptBox["qw", "2"]}]]]], "Output",
CellChangeTimes->{3.632651001671771*^9, 3.632651072007021*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"Expand", "[",
RowBox[{"Series", "[",
RowBox[{
FractionBox[
RowBox[{"2", " ",
RowBox[{"ArcCos", "[", "qw", "]"}]}],
SqrtBox[
RowBox[{"1", "-",
SuperscriptBox["qw", "2"]}]]], ",",
RowBox[{"{",
RowBox[{"qw", ",", "1", ",", "1"}], "}"}], ",",
RowBox[{"Assumptions", "->",
RowBox[{"(",
RowBox[{"qw", "<", "1"}], ")"}]}]}], "]"}], "]"}]], "Input",
CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, {
3.6326511716876993`*^9, 3.632651189491748*^9}, {3.632651248821335*^9,
3.632651267905816*^9}}],
Cell[BoxData[
InterpretationBox[
RowBox[{"2", "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"(",
RowBox[{"qw", "-", "1"}], ")"}]}], "3"], "+",
InterpretationBox[
SuperscriptBox[
RowBox[{"O", "[",
RowBox[{"qw", "-", "1"}], "]"}],
RowBox[{"3", "/", "2"}]],
SeriesData[$CellContext`qw, 1, {}, 0, 3, 2],
Editable->False]}],
SeriesData[$CellContext`qw, 1, {2, 0,
Rational[-2, 3]}, 0, 3, 2],
Editable->False]], "Output",
CellChangeTimes->{{3.632651102947558*^9, 3.632651118218814*^9}, {
3.632651179610784*^9, 3.6326511898522263`*^9}, {3.632651249719887*^9,
3.632651268312502*^9}}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"ArcCos", "[",
RowBox[{"-", "1"}], "]"}]], "Input",
CellChangeTimes->{{3.632651352754121*^9, 3.63265135286866*^9}}],
Cell[BoxData["\[Pi]"], "Output",
CellChangeTimes->{3.632651353300222*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"Expand", "[",
RowBox[{"Series", "[",
RowBox[{
FractionBox[
RowBox[{
RowBox[{"-", "2"}],
RowBox[{"ArcCos", "[",
RowBox[{"-", "qw"}], "]"}]}],
SqrtBox[
RowBox[{"1", "-",
SuperscriptBox["qw", "2"]}]]], ",",
RowBox[{"{",
RowBox[{"qw", ",",
RowBox[{"-", "1"}], ",", "1"}], "}"}], ",",
RowBox[{"Assumptions", "->",
RowBox[{"(",
RowBox[{"qw", ">",
RowBox[{"-", "1"}]}], ")"}]}]}], "]"}], "]"}]], "Input",
CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, {
3.6326511716876993`*^9, 3.6326512088422937`*^9}, {3.632651301817163*^9,
3.6326513406015453`*^9}, {3.63265150259446*^9, 3.632651505055284*^9}, {
3.632651744223112*^9, 3.632651772717318*^9}}],
Cell[BoxData[
InterpretationBox[
RowBox[{
RowBox[{"-", "2"}], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"(",
RowBox[{"qw", "+", "1"}], ")"}]}], "3"], "+",
InterpretationBox[
SuperscriptBox[
RowBox[{"O", "[",
RowBox[{"qw", "+", "1"}], "]"}],
RowBox[{"3", "/", "2"}]],
SeriesData[$CellContext`qw, -1, {}, 0, 3, 2],
Editable->False]}],
SeriesData[$CellContext`qw, -1, {-2, 0,
Rational[-2, 3]}, 0, 3, 2],
Editable->False]], "Output",
CellChangeTimes->{
3.632651209181905*^9, 3.6326513025091133`*^9, {3.632651332608609*^9,
3.632651341031022*^9}, 3.632651506516138*^9, {3.632651746679185*^9,
3.632651773032124*^9}}]
}, Open ]]
},
WindowSize->{808, 751},
WindowMargins->{{4, Automatic}, {Automatic, 4}},
FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (June 27, \
2014)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)
(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[558, 20, 263, 5, 49, "Text"],
Cell[CellGroupData[{
Cell[846, 29, 174, 4, 28, "Input"],
Cell[1023, 35, 154, 3, 28, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[1214, 43, 197, 5, 28, "Input"],
Cell[1414, 50, 129, 4, 40, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[1580, 59, 206, 4, 28, "Input"],
Cell[1789, 65, 233, 7, 59, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[2059, 77, 605, 17, 61, "Input"],
Cell[2667, 96, 645, 19, 48, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[3349, 120, 142, 3, 28, "Input"],
Cell[3494, 125, 74, 1, 28, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[3605, 131, 788, 22, 61, "Input"],
Cell[4396, 155, 692, 21, 48, "Output"]
}, Open ]]
}
]
*)
(* End of internal cache information *)

607
doc/Mathematica/dexpInvL_SE2.nb Normal file
View File

@ -0,0 +1,607 @@
(* Content-type: application/vnd.wolfram.mathematica *)
(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)
(* CreatedBy='Mathematica 8.0' *)
(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[ 157, 7]
NotebookDataLength[ 18933, 598]
NotebookOptionsPosition[ 18110, 565]
NotebookOutlinePosition[ 18464, 581]
CellTagsIndexPosition[ 18421, 578]
WindowFrame->Normal*)
(* Beginning of Notebook Content *)
Notebook[{
Cell[TextData[{
"The \[OpenCurlyQuote]right trivialised\[CloseCurlyQuote] tangent of the \
exponential map, ",
Cell[BoxData[
FormBox["dexpR", TraditionalForm]],
FormatType->"TraditionalForm"],
", according to Iserles05an, formula 2.42, pg. 32 can be written as\n\t",
Cell[BoxData[
FormBox[GridBox[{
{"\t"},
{
RowBox[{
RowBox[{
RowBox[{"g", "'"}],
SuperscriptBox["g",
RowBox[{"-", "1"}]]}], "=",
RowBox[{
SubscriptBox["dexpR", "\[Omega]"], "(",
RowBox[{"\[Omega]", "'"}], ")"}]}]}
}], TraditionalForm]],
FormatType->"TraditionalForm"],
"\nwhere ",
Cell[BoxData[
FormBox[
RowBox[{"g", "=",
RowBox[{"exp", "(", "\[Omega]", ")"}]}], TraditionalForm]],
FormatType->"TraditionalForm"],
", and ",
Cell[BoxData[
FormBox[
RowBox[{
RowBox[{"g", "'"}], "=",
RowBox[{
RowBox[{"exp", "'"}],
RowBox[{"(", "\[Omega]", ")"}]}]}], TraditionalForm]],
FormatType->"TraditionalForm"],
".\nCompare this to the left Jacobian matrix ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "l"], TraditionalForm]],
FormatType->"TraditionalForm"],
" in Chirikjian11book2, pg. 26, we see that ",
Cell[BoxData[
FormBox["dexpR", TraditionalForm]],
FormatType->"TraditionalForm"],
" and ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "l"], TraditionalForm]],
FormatType->"TraditionalForm"],
" are the same.\n\nHence, be careful, Iserles\[CloseCurlyQuote]s \
\[OpenCurlyQuote]",
StyleBox["right",
FontWeight->"Bold"],
" trivialised\[CloseCurlyQuote] tangent of the exponential map ",
Cell[BoxData[
FormBox["dexpR", TraditionalForm]],
FormatType->"TraditionalForm"],
" is in fact Chirikjian\[CloseCurlyQuote]s ",
StyleBox["left",
FontWeight->"Bold"],
" Jacobian matrix ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "l"], TraditionalForm]],
FormatType->"TraditionalForm"],
"!!!\n\nWe want to compute the s \[OpenCurlyQuote]",
StyleBox["left",
FontWeight->"Bold"],
" trivialised\[CloseCurlyQuote] tangent of the exponential map, ",
Cell[BoxData[
FormBox["dexpL", TraditionalForm]],
FormatType->"TraditionalForm"],
", for SE2, hence, we need to use Chirikjian\[CloseCurlyQuote]s ",
StyleBox["right",
FontWeight->"Bold"],
" Jacobian matrix ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "r"], TraditionalForm]],
FormatType->"TraditionalForm"],
" formula in Chirikjian11book2, pg. 36."
}], "Text",
CellChangeTimes->{{3.6279967389044943`*^9, 3.6279968865058002`*^9}, {
3.6279969695759087`*^9, 3.6279974871811028`*^9}, 3.62799757389325*^9}],
Cell[BoxData[{
RowBox[{"Clear", "[", "J", "]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"J", "[", "\[Alpha]_", "]"}], ":=",
RowBox[{"{",
RowBox[{
RowBox[{"{",
RowBox[{
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",",
RowBox[{
RowBox[{"(",
RowBox[{"1", "-",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], "/", "\[Alpha]"}],
",", " ",
RowBox[{
RowBox[{"(",
RowBox[{
RowBox[{"\[Alpha]", " ",
SubscriptBox["v", "1"]}], "-",
SubscriptBox["v", "2"], "+",
RowBox[{
SubscriptBox["v", "2"],
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-",
RowBox[{
SubscriptBox["v", "1"],
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/",
SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",",
RowBox[{"{",
RowBox[{
RowBox[{
RowBox[{"-",
RowBox[{"(",
RowBox[{"1", "-",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}], "/", "\[Alpha]"}],
",",
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",", " ",
RowBox[{
RowBox[{"(",
RowBox[{
SubscriptBox["v", "1"], "+",
RowBox[{"\[Alpha]", " ",
SubscriptBox["v", "2"]}], "-",
RowBox[{
SubscriptBox["v", "1"],
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-",
RowBox[{
SubscriptBox["v", "2"],
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/",
SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",",
RowBox[{"{",
RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}]}], "Input",
CellChangeTimes->{{3.627993817228732*^9, 3.6279939547434673`*^9}, {
3.627993986274671*^9, 3.6279940386007967`*^9}, {3.627995391081044*^9,
3.627995412846286*^9}, 3.6279954452391644`*^9, {3.627995531089571*^9,
3.6279955341932592`*^9}, {3.627996429604282*^9, 3.62799643077184*^9}}],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Jinv", "[", "\[Alpha]_", "]"}], "=",
RowBox[{"Inverse", "[",
RowBox[{"J", "[", "\[Alpha]", "]"}], "]"}]}],
"\[IndentingNewLine]"}]], "Input",
CellChangeTimes->{
3.627995475343099*^9, {3.627995548533964*^9, 3.627995559455151*^9}, {
3.627996438504909*^9, 3.6279964431657553`*^9}}],
Cell[BoxData[
RowBox[{"{",
RowBox[{
RowBox[{"{",
RowBox[{
FractionBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}],
RowBox[{"\[Alpha]", " ",
RowBox[{"(",
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",",
FractionBox[
RowBox[{
RowBox[{"-",
FractionBox["1", "\[Alpha]"]}], "+",
FractionBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]], ",",
FractionBox[
RowBox[{
FractionBox[
SubscriptBox["v", "1"],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "3"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "3"]], "+",
FractionBox[
SubscriptBox["v", "2"],
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "2"]]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",",
RowBox[{"{",
RowBox[{
FractionBox[
RowBox[{
FractionBox["1", "\[Alpha]"], "-",
FractionBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]], ",",
FractionBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}],
RowBox[{"\[Alpha]", " ",
RowBox[{"(",
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",",
FractionBox[
RowBox[{
RowBox[{"-",
FractionBox[
SubscriptBox["v", "1"],
SuperscriptBox["\[Alpha]", "2"]]}], "+",
FractionBox[
RowBox[{
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SubscriptBox["v", "2"],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "3"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "3"]]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",",
RowBox[{"{",
RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]], "Output",
CellChangeTimes->{
3.627995560030972*^9, {3.627996412919798*^9, 3.627996444306521*^9}}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Jinv", "[", "\[Alpha]", "]"}], "//", "Simplify"}], "//",
"MatrixForm"}]], "Input",
CellChangeTimes->{{3.627993835637863*^9, 3.627993839233502*^9}, {
3.627994046108457*^9, 3.627994058781851*^9}, {3.627995546842499*^9,
3.6279955664940767`*^9}}],
Cell[BoxData[
TagBox[
RowBox[{"(", "\[NoBreak]", GridBox[{
{
RowBox[{
FractionBox["1", "2"], " ", "\[Alpha]", " ",
RowBox[{"Cot", "[",
FractionBox["\[Alpha]", "2"], "]"}]}],
RowBox[{"-",
FractionBox["\[Alpha]", "2"]}],
FractionBox[
RowBox[{
RowBox[{
RowBox[{"(",
RowBox[{
RowBox[{"-", "2"}], "+",
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+",
RowBox[{"\[Alpha]", " ",
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ",
SubscriptBox["v", "1"]}], "+",
RowBox[{"\[Alpha]", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-", "1"}], "+",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], " ",
SubscriptBox["v", "2"]}]}],
RowBox[{"2", " ", "\[Alpha]", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-", "1"}], "+",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]},
{
FractionBox["\[Alpha]", "2"],
RowBox[{
FractionBox["1", "2"], " ", "\[Alpha]", " ",
RowBox[{"Cot", "[",
FractionBox["\[Alpha]", "2"], "]"}]}],
FractionBox[
RowBox[{
RowBox[{
RowBox[{"(",
RowBox[{"\[Alpha]", "-",
RowBox[{"\[Alpha]", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}]}], ")"}], " ",
SubscriptBox["v", "1"]}], "+",
RowBox[{
RowBox[{"(",
RowBox[{
RowBox[{"-", "2"}], "+",
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+",
RowBox[{"\[Alpha]", " ",
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ",
SubscriptBox["v", "2"]}]}],
RowBox[{"2", " ", "\[Alpha]", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-", "1"}], "+",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]},
{"0", "0", "1"}
},
GridBoxAlignment->{
"Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
"RowsIndexed" -> {}},
GridBoxSpacings->{"Columns" -> {
Offset[0.27999999999999997`], {
Offset[0.7]},
Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
Offset[0.2], {
Offset[0.4]},
Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
Function[BoxForm`e$,
MatrixForm[BoxForm`e$]]]], "Output",
CellChangeTimes->{
3.627993840513033*^9, {3.62799404156531*^9, 3.6279940592345743`*^9},
3.627995567356995*^9, 3.627996415136314*^9, 3.6279964490074778`*^9}]
}, Open ]],
Cell[TextData[{
"In case ",
Cell[BoxData[
FormBox[
RowBox[{"\[Alpha]", "=", "0"}], TraditionalForm]],
FormatType->"TraditionalForm"],
", we compute the limits of ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "r"], TraditionalForm]],
FormatType->"TraditionalForm"],
" and ",
Cell[BoxData[
FormBox[
SuperscriptBox[
SubscriptBox["J", "r"],
RowBox[{"-", "1"}]], TraditionalForm]],
FormatType->"TraditionalForm"],
" as follows"
}], "Text",
CellChangeTimes->{{3.627997495449997*^9, 3.627997524522543*^9}}],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Limit", "[",
RowBox[{
RowBox[{"Jinv", "[", "\[Alpha]", "]"}], ",",
RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//",
"MatrixForm"}]], "Input",
CellChangeTimes->{{3.627995572179821*^9, 3.627995606373824*^9}}],
Cell[BoxData[
TagBox[
RowBox[{"(", "\[NoBreak]", GridBox[{
{"1", "0",
FractionBox[
SubscriptBox["v", "2"], "2"]},
{"0", "1",
RowBox[{"-",
FractionBox[
SubscriptBox["v", "1"], "2"]}]},
{"0", "0", "1"}
},
GridBoxAlignment->{
"Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
"RowsIndexed" -> {}},
GridBoxSpacings->{"Columns" -> {
Offset[0.27999999999999997`], {
Offset[0.7]},
Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
Offset[0.2], {
Offset[0.4]},
Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
Function[BoxForm`e$,
MatrixForm[BoxForm`e$]]]], "Output",
CellChangeTimes->{{3.627995585954463*^9, 3.627995606858135*^9},
3.6279964178087473`*^9, 3.6279964634008904`*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Limit", "[",
RowBox[{
RowBox[{"J", "[", "\[Alpha]", "]"}], ",",
RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//",
"MatrixForm"}]], "Input",
CellChangeTimes->{{3.6279956527343893`*^9, 3.627995660697241*^9}}],
Cell[BoxData[
TagBox[
RowBox[{"(", "\[NoBreak]", GridBox[{
{"1", "0",
RowBox[{"-",
FractionBox[
SubscriptBox["v", "2"], "2"]}]},
{"0", "1",
FractionBox[
SubscriptBox["v", "1"], "2"]},
{"0", "0", "1"}
},
GridBoxAlignment->{
"Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
"RowsIndexed" -> {}},
GridBoxSpacings->{"Columns" -> {
Offset[0.27999999999999997`], {
Offset[0.7]},
Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
Offset[0.2], {
Offset[0.4]},
Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
Function[BoxForm`e$,
MatrixForm[BoxForm`e$]]]], "Output",
CellChangeTimes->{{3.627995653969245*^9, 3.627995661346282*^9},
3.627996419383007*^9, 3.627996465705708*^9}]
}, Open ]],
Cell[BoxData[""], "Input",
CellChangeTimes->{{3.627995694633294*^9, 3.627995695945466*^9}}]
},
WindowSize->{654, 852},
WindowMargins->{{Automatic, 27}, {Automatic, 0}},
FrontEndVersion->"8.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (October 5, \
2011)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)
(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[557, 20, 2591, 84, 197, "Text"],
Cell[3151, 106, 2022, 56, 68, "Input"],
Cell[CellGroupData[{
Cell[5198, 166, 343, 9, 43, "Input"],
Cell[5544, 177, 6519, 190, 290, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[12100, 372, 298, 7, 27, "Input"],
Cell[12401, 381, 2665, 76, 99, "Output"]
}, Open ]],
Cell[15081, 460, 535, 20, 29, "Text"],
Cell[CellGroupData[{
Cell[15641, 484, 297, 8, 27, "Input"],
Cell[15941, 494, 863, 25, 91, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[16841, 524, 296, 8, 27, "Input"],
Cell[17140, 534, 859, 25, 91, "Output"]
}, Open ]],
Cell[18014, 562, 92, 1, 27, "Input"]
}
]
*)
(* End of internal cache information *)

64
examples/METISOrderingExample.cpp Normal file
View File

@ -0,0 +1,64 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file METISOrdering.cpp
* @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
* @author Frank Dellaert
* @author Andrew Melim
*/
/**
* Example of a simple 2D localization example optimized using METIS ordering
* - For more details on the full optimization pipeline, see OdometryExample.cpp
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
NonlinearFactorGraph graph;
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.print("\nFactor Graph:\n"); // print
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
initial.print("\nInitial Estimate:\n"); // print
// optimize using Levenberg-Marquardt optimization
LevenbergMarquardtParams params;
// In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
// By default this parameter is set to OrderingType::COLAMD
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
Values result = optimizer.optimize();
result.print("Final Result:\n");
return 0;
}

28
gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp → examples/Pose2SLAMExampleExpressions.cpp
View File

@ -14,20 +14,18 @@
* @brief Expressions version of Pose2SLAMExample.cpp
* @date Oct 2, 2014
* @author Frank Dellaert
* @author Yong Dian Jian
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.h>
using namespace std;
using namespace gtsam;
@ -35,26 +33,26 @@ using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
ExpressionFactorGraph graph;
// Create Expressions for unknowns
Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(ExpressionFactor<Pose2>(priorNoise, Pose2(0, 0, 0), x1));
graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, 0 ), between(x1,x2)));
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x2,x3)));
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x3,x4)));
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x4,x5)));
graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model);
graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x5,x2)));
graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution

1
examples/Pose2SLAMExample_g2o.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>

10
examples/Pose2SLAMExample_graph.cpp
View File

@ -16,11 +16,15 @@
* @author Frank Dellaert
*/
#include <gtsam/slam/dataset.h>
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
// This new header allows us to read examples easily from .graph files
#include <gtsam/slam/dataset.h>
using namespace std;
using namespace gtsam;

4
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -16,11 +16,11 @@
* @author Frank Dellaert
*/
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <fstream>
using namespace std;

1
examples/Pose2SLAMExample_lago.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <fstream>
using namespace std;

69
examples/Pose2SLAMwSPCG.cpp
View File

@ -16,47 +16,15 @@
* @date June 2, 2012
*/
/**
* A simple 2D pose slam example solved using a Conjugate-Gradient method
* - The robot moves in a 2 meter square
* - The robot moves 2 meters each step, turning 90 degrees after each step
* - The robot initially faces along the X axis (horizontal, to the right in 2D)
* - We have full odometry between pose
* - We have a loop closure constraint when the robot returns to the first position
*/
// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent
// the robot positions
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
// Each variable in the system (poses) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use simple integer keys
#include <gtsam/inference/Key.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// We will also use a Between Factor to encode the loop closure constraint
// Also, we will initialize the robot at the origin using a Prior factor.
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// In contrast to that example, however, we will use a PCG solver here
#include <gtsam/linear/SubgraphSolver.h>
using namespace std;
using namespace gtsam;
@ -66,32 +34,24 @@ int main(int argc, char** argv) {
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
// 2b. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images.
// We will use another Between Factor to enforce this constraint, with the distance set to zero,
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
@ -104,15 +64,18 @@ int main(int argc, char** argv) {
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
{
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
cout << "subgraph solver final error = " << graph.error(result) << endl;
}
// LM is still the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
cout << "subgraph solver final error = " << graph.error(result) << endl;
return 0;
}

3
examples/RangeISAMExample_plaza2.cpp
View File

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

8
examples/SFMExample.cpp
View File

@ -15,13 +15,7 @@
* @author Duy-Nguyen Ta
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// For loading the data
// For loading the data, see the comments therein for scenario (camera rotates around cube)
#include "SFMdata.h"
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).

38
gtsam_unstable/examples/SFMExampleExpressions.cpp → examples/SFMExampleExpressions.cpp
View File

@ -23,15 +23,12 @@
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <examples/SFMdata.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
@ -40,27 +37,29 @@
using namespace std;
using namespace gtsam;
using namespace noiseModel;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
Isotropic::shared_ptr measurementNoise = Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
vector<Point3> points = createPoints();
vector<Pose3> poses = createPoses();
// Create a factor graph
NonlinearFactorGraph graph;
ExpressionFactorGraph graph;
// Specify uncertainty on first pose prior
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
Vector6 sigmas; sigmas << Vector3(0.3,0.3,0.3), Vector3(0.1,0.1,0.1);
Diagonal::shared_ptr poseNoise = Diagonal::Sigmas(sigmas);
// Here we don't use a PriorFactor but directly the ExpressionFactor class
// The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
// x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
Pose3_ x0('x',0);
graph.push_back(ExpressionFactor<Pose3>(poseNoise, poses[0], x0));
graph.addExpressionFactor(x0, poses[0], poseNoise);
// We create a constant Expression for the calibration here
Cal3_S2_ cK(K);
@ -74,25 +73,26 @@ int main(int argc, char* argv[]) {
// Below an expression for the prediction of the measurement:
Point3_ p('l', j);
Point2_ prediction = uncalibrate(cK, project(transform_to(x, p)));
// Again, here we use a ExpressionFactor
graph.push_back(ExpressionFactor<Point2>(measurementNoise, measurement, prediction));
// Again, here we use an ExpressionFactor
graph.addExpressionFactor(prediction, measurement, measurementNoise);
}
}
// Add prior on first point to constrain scale, again with ExpressionFactor
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(ExpressionFactor<Point3>(pointNoise, points[0], Point3_('l', 0)));
Isotropic::shared_ptr pointNoise = Isotropic::Sigma(3, 0.1);
graph.addExpressionFactor(Point3_('l', 0), points[0], pointNoise);
// Create perturbed initial
Values initialEstimate;
Values initial;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
initial.insert(Symbol('x', i), poses[i].compose(delta));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
cout << "initial error = " << graph.error(initialEstimate) << endl;
initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
cout << "initial error = " << graph.error(initial) << endl;
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
Values result = DoglegOptimizer(graph, initial).optimize();
cout << "final error = " << graph.error(result) << endl;
return 0;

121
examples/SFMExampleExpressions_bal.cpp Normal file
View File

@ -0,0 +1,121 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SFMExampleExpressions_bal.cpp
* @brief A structure-from-motion example done with Expressions
* @author Frank Dellaert
* @date January 2015
*/
/**
* This is the Expression version of SFMExample
* See detailed description of headers there, this focuses on explaining the AD part
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <vector>
using namespace std;
using namespace gtsam;
using namespace noiseModel;
using symbol_shorthand::C;
using symbol_shorthand::P;
// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc > 1)
filename = string(argv[1]);
// Load the SfM data from file
SfM_data mydata;
readBAL(filename, mydata);
cout
<< boost::format("read %1% tracks on %2% cameras\n")
% mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
ExpressionFactorGraph graph;
// Here we don't use a PriorFactor but directly the ExpressionFactor class
// First, we create an expression to the pose from the first camera
Expression<SfM_Camera> camera0_(C(0));
// Then, to get its pose:
Pose3_ pose0_(&SfM_Camera::getPose, camera0_);
// Finally, we say it should be equal to first guess
graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
noiseModel::Isotropic::Sigma(6, 0.1));
// similarly, we create a prior on the first point
Point3_ point0_(P(0));
graph.addExpressionFactor(point0_, mydata.tracks[0].p,
noiseModel::Isotropic::Sigma(3, 0.1));
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
1.0); // one pixel in u and v
// Simulated measurements from each camera pose, adding them to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
// Leaf expression for j^th point
Point3_ point_('p', j);
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
// Leaf expression for i^th camera
Expression<SfM_Camera> camera_(C(i));
// Below an expression for the prediction of the measurement:
Point2_ predict_ = project2<SfM_Camera>(camera_, point_);
// Again, here we use an ExpressionFactor
graph.addExpressionFactor(predict_, uv, noise);
}
j += 1;
}
// Create initial estimate
Values initial;
size_t i = 0;
j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras)
initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks)
initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;
try {
LevenbergMarquardtParams params;
params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer lm(graph, initial, params);
result = lm.optimize();
} catch (exception& e) {
cout << e.what();
}
cout << "final error: " << graph.error(result) << endl;
return 0;
}
/* ************************************************************************* */

59
examples/SFMExample_SmartFactor.cpp
View File

@ -17,52 +17,22 @@
* @author Frank Dellaert
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// For loading the data
#include "SFMdata.h"
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
#include <gtsam/geometry/Point2.h>
// In GTSAM, measurement functions are represented as 'factors'.
// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark,
// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration,
// The calibration should be known.
// The factor we used here is SmartProjectionPoseFactor.
// Every smart factor represent a single landmark, seen from multiple cameras.
// The SmartProjectionPoseFactor only optimizes for the poses of a camera,
// not the calibration, which is assumed known.
#include <gtsam/slam/SmartProjectionPoseFactor.h>
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
// trust-region method known as Powell's Degleg
#include <gtsam/nonlinear/DoglegOptimizer.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
#include <vector>
// For an explanation of these headers, see SFMExample.cpp
#include "SFMdata.h"
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3,
gtsam::Cal3_S2> SmartFactor;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
@ -128,7 +98,8 @@ int main(int argc, char* argv[]) {
initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
result.print("Final results:\n");
// A smart factor represent the 3D point inside the factor, not as a variable.
@ -137,20 +108,20 @@ int main(int argc, char* argv[]) {
Values landmark_result;
for (size_t j = 0; j < points.size(); ++j) {
// The output of point() is in boost::optional<gtsam::Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point;
// The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
point = smart->point(result);
// The output of point() is in boost::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
landmark_result.insert(j, *point);
}
}
landmark_result.print("Landmark results:\n");
cout << "final error: " << graph.error(result) << endl;
cout << "number of iterations: " << optimizer.iterations() << endl;
return 0;
}

126
examples/SFMExample_SmartFactorPCG.cpp Normal file
View File

@ -0,0 +1,126 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SFMExample_SmartFactorPCG.cpp
* @brief Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient
* @author Frank Dellaert
*/
// For an explanation of these headers, see SFMExample_SmartFactor.cpp
#include "SFMdata.h"
#include <gtsam/slam/SmartProjectionPoseFactor.h>
// These extra headers allow us a LM outer loop with PCG linear solver (inner loop)
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/PCGSolver.h>
using namespace std;
using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
vector<Point3> points = createPoints();
vector<Pose3> poses = createPoses();
// Create a factor graph
NonlinearFactorGraph graph;
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor());
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
smartfactor->add(measurement, i, measurementNoise, K);
}
// insert the smart factor in the graph
graph.push_back(smartfactor);
}
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
// Fix the scale ambiguity by adding a prior
graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise));
// Create the initial estimate to the solution
Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
LevenbergMarquardtParams parameters;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
parameters.absoluteErrorTol = 1e-10;
parameters.relativeErrorTol = 1e-10;
parameters.maxIterations = 500;
PCGSolverParameters::shared_ptr pcg =
boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ =
boost::make_shared<BlockJacobiPreconditionerParameters>();
// Following is crucial:
pcg->setEpsilon_abs(1e-10);
pcg->setEpsilon_rel(1e-10);
parameters.iterativeParams = pcg;
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
// Display result as in SFMExample_SmartFactor.run
result.print("Final results:\n");
Values landmark_result;
for (size_t j = 0; j < points.size(); ++j) {
SmartFactor::shared_ptr smart = //
boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
landmark_result.insert(j, *point);
}
}
landmark_result.print("Landmark results:\n");
cout << "final error: " << graph.error(result) << endl;
cout << "number of iterations: " << optimizer.iterations() << endl;
return 0;
}
/* ************************************************************************* */

2
examples/SFMExample_bal.cpp
View File

@ -43,7 +43,7 @@ int main (int argc, char* argv[]) {
// Load the SfM data from file
SfM_data mydata;
assert(readBAL(filename, mydata));
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph

56
examples/SolverComparer.cpp
View File

@ -31,28 +31,29 @@
*
*/
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <fstream>
#include <iostream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/random.hpp>
#include <boost/serialization/export.hpp>
#include <fstream>
#include <iostream>
#ifdef GTSAM_USE_TBB
#include <tbb/tbb.h>
@ -72,23 +73,6 @@ typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR;
BOOST_CLASS_EXPORT(Value);
BOOST_CLASS_EXPORT(Pose);
BOOST_CLASS_EXPORT(Rot2);
BOOST_CLASS_EXPORT(Point2);
BOOST_CLASS_EXPORT(NonlinearFactor);
BOOST_CLASS_EXPORT(NoiseModelFactor);
BOOST_CLASS_EXPORT(NM1);
BOOST_CLASS_EXPORT(NM2);
BOOST_CLASS_EXPORT(BetweenFactor<Pose>);
BOOST_CLASS_EXPORT(PriorFactor<Pose>);
BOOST_CLASS_EXPORT(BR);
BOOST_CLASS_EXPORT(noiseModel::Base);
BOOST_CLASS_EXPORT(noiseModel::Isotropic);
BOOST_CLASS_EXPORT(noiseModel::Gaussian);
BOOST_CLASS_EXPORT(noiseModel::Diagonal);
BOOST_CLASS_EXPORT(noiseModel::Unit);
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
// Compute degrees of freedom (observations - variables)
// In ocaml, +1 was added to the observations to account for the prior, but
@ -269,12 +253,12 @@ void runIncremental()
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
{
Key key1 = measurement->key1(), key2 = measurement->key2();
if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) {
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
// We found an odometry starting at firstStep
firstPose = std::min(key1, key2);
break;
}
if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) {
if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) {
// We found an odometry joining firstStep with a previous pose
havePreviousPose = true;
firstPose = std::max(key1, key2);
@ -295,7 +279,7 @@ void runIncremental()
NonlinearFactorGraph newFactors;
Values newVariables;
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(Pose::Dim())));
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(3)));
newVariables.insert(firstPose, Pose());
isam2.update(newFactors, newVariables);
@ -303,7 +287,9 @@ void runIncremental()
cout << "Playing forward time steps..." << endl;
for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step)
for (size_t step = firstPose;
nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep);
++step)
{
Values newVariables;
NonlinearFactorGraph newFactors;
@ -474,7 +460,7 @@ void runBatch()
cout << "Creating batch optimizer..." << endl;
NonlinearFactorGraph measurements = datasetMeasurements;
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(3)));
gttic_(Create_optimizer);
GaussNewtonParams params;
@ -589,7 +575,7 @@ void runStats()
{
cout << "Gathering statistics..." << endl;
GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear)));
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::colamd(linear)));
treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
ofstream file;

4
examples/StereoVOExample_large.cpp
View File

@ -103,7 +103,9 @@ int main(int argc, char** argv){
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
LevenbergMarquardtParams params;
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;

8
examples/TimeTBB.cpp
View File

@ -134,7 +134,7 @@ map<int, double> testWithMemoryAllocation()
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
tbb::tick_count t1 = tbb::tick_count::now();
cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
timingResults[grainSize] = (t1 - t0).seconds();
timingResults[(int)grainSize] = (t1 - t0).seconds();
}
return timingResults;
@ -152,9 +152,9 @@ int main(int argc, char* argv[])
BOOST_FOREACH(size_t n, numThreads)
{
cout << "With " << n << " threads:" << endl;
tbb::task_scheduler_init init(n);
results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[n].grainSizesWithAllocation = testWithMemoryAllocation();
tbb::task_scheduler_init init((int)n);
results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
cout << endl;
}

437
gtsam.h
View File

@ -156,13 +156,7 @@ virtual class Value {
size_t dim() const;
};
class Vector3 {
Vector3(Vector v);
};
class Vector6 {
Vector6(Vector v);
};
#include <gtsam/base/LieScalar.h>
#include <gtsam/base/LieScalar_Deprecated.h>
class LieScalar {
// Standard constructors
LieScalar();
@ -191,7 +185,7 @@ class LieScalar {
static Vector Logmap(const gtsam::LieScalar& p);
};
#include <gtsam/base/LieVector.h>
#include <gtsam/base/LieVector_Deprecated.h>
class LieVector {
// Standard constructors
LieVector();
@ -223,7 +217,7 @@ class LieVector {
void serialize() const;
};
#include <gtsam/base/LieMatrix.h>
#include <gtsam/base/LieMatrix_Deprecated.h>
class LieMatrix {
// Standard constructors
LieMatrix();
@ -276,8 +270,6 @@ class Point2 {
gtsam::Point2 between(const gtsam::Point2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Point2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point2& p) const;
@ -296,6 +288,32 @@ class Point2 {
void serialize() const;
};
// std::vector<gtsam::Point2>
class Point2Vector
{
// Constructors
Point2Vector();
Point2Vector(const gtsam::Point2Vector& v);
//Capacity
size_t size() const;
size_t max_size() const;
void resize(size_t sz);
size_t capacity() const;
bool empty() const;
void reserve(size_t n);
//Element access
gtsam::Point2 at(size_t n) const;
gtsam::Point2 front() const;
gtsam::Point2 back() const;
//Modifiers
void assign(size_t n, const gtsam::Point2& u);
void push_back(const gtsam::Point2& x);
void pop_back();
};
class StereoPoint2 {
// Standard Constructors
StereoPoint2();
@ -312,8 +330,6 @@ class StereoPoint2 {
gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::StereoPoint2 retract(Vector v) const;
Vector localCoordinates(const gtsam::StereoPoint2& p) const;
@ -348,8 +364,6 @@ class Point3 {
gtsam::Point3 between(const gtsam::Point3& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Point3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point3& p) const;
@ -386,8 +400,6 @@ class Rot2 {
gtsam::Rot2 between(const gtsam::Rot2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Rot2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot2& p) const;
@ -439,8 +451,6 @@ class Rot3 {
gtsam::Rot3 between(const gtsam::Rot3& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
//gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const;
@ -488,8 +498,6 @@ class Pose2 {
gtsam::Pose2 between(const gtsam::Pose2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Pose2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Pose2& p) const;
@ -537,10 +545,7 @@ class Pose3 {
gtsam::Pose3 between(const gtsam::Pose3& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Pose3 retract(Vector v) const;
gtsam::Pose3 retractFirstOrder(Vector v) const;
Vector localCoordinates(const gtsam::Pose3& T2) const;
// Lie Group
@ -569,6 +574,16 @@ class Pose3 {
void serialize() const;
};
// std::vector<gtsam::Pose3>
class Pose3Vector
{
Pose3Vector();
size_t size() const;
bool empty() const;
gtsam::Pose3 at(size_t n) const;
void push_back(const gtsam::Pose3& x);
};
#include <gtsam/geometry/Unit3.h>
class Unit3 {
// Standard Constructors
@ -648,28 +663,13 @@ class Cal3_S2 {
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
class Cal3DS2 {
#include <gtsam/geometry/Cal3DS2_Base.h>
virtual class Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
Cal3DS2(Vector v);
Cal3DS2_Base();
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// TODO: D2d functions that start with an uppercase letter
// Standard Interface
double fx() const;
@ -677,14 +677,66 @@ class Cal3DS2 {
double skew() const;
double px() const;
double py() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
double k1() const;
double k2() const;
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
Cal3DS2(Vector v);
// Testable
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3Unified.h>
virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3Unified();
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
Cal3Unified(Vector v);
// Testable
bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
// Standard Interface
double xi() const;
gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3Unified retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Unified& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3_S2Stereo.h>
class Cal3_S2Stereo {
// Standard Constructors
Cal3_S2Stereo();
@ -758,10 +810,6 @@ class CalibratedCamera {
gtsam::CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
// Group
gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
gtsam::CalibratedCamera inverse() const;
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
@ -774,56 +822,16 @@ class CalibratedCamera {
void serialize() const;
};
class SimpleCamera {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K,
const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye,
const gtsam::Point3& target, const gtsam::Point3& upVector,
const gtsam::Cal3_S2& K);
// Testable
void print(string s) const;
bool equals(const gtsam::SimpleCamera& camera, double tol) const;
// Standard Interface
gtsam::Pose3 pose() const;
gtsam::Cal3_S2 calibration();
// Manifold
gtsam::SimpleCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
size_t dim() const;
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
double range(const gtsam::Point3& point);
double range(const gtsam::Pose3& point);
// enabling serialization functionality
void serialize() const;
};
template<CALIBRATION = {gtsam::Cal3DS2}>
template<CALIBRATION>
class PinholeCamera {
// Standard Constructors and Named Constructors
PinholeCamera();
PinholeCamera(const gtsam::Pose3& pose);
PinholeCamera(const gtsam::Pose3& pose, const gtsam::Cal3DS2& K);
static This Level(const gtsam::Cal3DS2& K,
const gtsam::Pose2& pose, double height);
PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height);
static This Level(const gtsam::Pose2& pose, double height);
static This Lookat(const gtsam::Point3& eye,
const gtsam::Point3& target, const gtsam::Point3& upVector,
const gtsam::Cal3DS2& K);
static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
const gtsam::Point3& upVector, const CALIBRATION& K);
// Testable
void print(string s) const;
@ -851,6 +859,50 @@ class PinholeCamera {
void serialize() const;
};
virtual class SimpleCamera {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
// Testable
void print(string s) const;
bool equals(const gtsam::SimpleCamera& camera, double tol) const;
// Standard Interface
gtsam::Pose3 pose() const;
gtsam::Cal3_S2 calibration() const;
// Manifold
gtsam::SimpleCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
size_t dim() const;
static size_t Dim();
// Transformations and measurement functions
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
double range(const gtsam::Point3& point);
double range(const gtsam::Pose3& point);
// enabling serialization functionality
void serialize() const;
};
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
class StereoCamera {
// Standard Constructors and Named Constructors
StereoCamera();
@ -879,6 +931,16 @@ class StereoCamera {
void serialize() const;
};
#include <gtsam/geometry/triangulation.h>
// Templates appear not yet supported for free functions
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
//*************************************************************************
// Symbolic
//*************************************************************************
@ -1234,6 +1296,7 @@ class VectorValues {
#include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor {
gtsam::KeyVector keys() const;
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
@ -1657,6 +1720,7 @@ class NonlinearFactorGraph {
void push_back(gtsam::NonlinearFactor* factor);
void add(gtsam::NonlinearFactor* factor);
bool exists(size_t idx) const;
gtsam::KeySet keys() const;
// NonlinearFactorGraph
double error(const gtsam::Values& values) const;
@ -1727,6 +1791,7 @@ class Values {
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
void insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
@ -1737,7 +1802,13 @@ class Values {
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
void insert(size_t j, const gtsam::SimpleCamera& t);
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
// Fixed size version
void insertFixed(size_t j, Vector t, size_t n);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
@ -1750,10 +1821,21 @@ class Values {
void update(size_t j, const gtsam::Cal3Bundler& t);
void update(size_t j, const gtsam::EssentialMatrix& t);
void update(size_t j, const gtsam::imuBias::ConstantBias& t);
void update(size_t j, Vector t);
void update(size_t j, Matrix t);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::imuBias::ConstantBias}>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
T at(size_t j);
/// Fixed size versions, for n in 1..9
void insertFixed(size_t j, Vector t, size_t n);
/// 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;
};
// Actually a FastList<Key>
@ -2142,15 +2224,12 @@ class NonlinearISAM {
//*************************************************************************
// Nonlinear factor types
//*************************************************************************
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2195,10 +2274,14 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
// Commented out by Frank Dec 2014: not poses!
// If needed, we need a RangeFactor that takes a camera, extracts the pose
// Should be easy with Expressions
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/BearingFactor.h>
@ -2272,23 +2355,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template<POSE, LANDMARK, CALIBRATION>
template<CALIBRATION>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
SmartProjectionPoseFactor(double rankTol, double linThreshold,
bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor);
bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(double rankTol);
SmartProjectionPoseFactor();
void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i,
const CALIBRATION* K_i);
void add(const gtsam::Point2& measured_i, size_t poseKey_i,
const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i);
// enabling serialization functionality
//void serialize() const;
};
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2> SmartProjectionPose3Factor;
#include <gtsam/slam/StereoFactor.h>
@ -2370,8 +2453,6 @@ class ConstantBias {
gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::imuBias::ConstantBias retract(Vector v) const;
Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
@ -2390,25 +2471,24 @@ class ConstantBias {
}///\namespace imuBias
#include <gtsam/navigation/ImuFactor.h>
class PoseVelocity{
PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity);
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Matrix deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHat() const;
Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
@ -2416,10 +2496,11 @@ class ImuFactorPreintegratedMeasurements {
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
};
virtual class ImuFactor : gtsam::NonlinearFactor {
@ -2430,16 +2511,65 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
};
#include <gtsam/navigation/CombinedImuFactor.h>
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
// CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
double deltaTij() const;
Matrix 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);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) 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::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
};
#include <gtsam/navigation/AHRSFactor.h>
class AHRSFactorPreintegratedMeasurements {
// Standard Constructor
AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs);
// Testable
@ -2447,7 +2577,6 @@ class AHRSFactorPreintegratedMeasurements {
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
// get Data
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
@ -2468,70 +2597,12 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
// Standard Interface
gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
const gtsam::imuBias::ConstantBias& bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, const gtsam::imuBias::ConstantBias& bias,
Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const;
};
#include <gtsam/navigation/CombinedImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
};
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHat() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
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::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
};
#include <gtsam/navigation/AttitudeFactor.h>
//virtual class AttitudeFactor : gtsam::NonlinearFactor {
// AttitudeFactor(const Unit3& nZ, const Unit3& bRef);

20
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -18,7 +18,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
# do the same for the unsupported eigen folder
file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*")
foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all})
get_filename_component(filename ${unsupported_eigen_dir} NAME)
@ -36,17 +36,17 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
install(DIRECTORY Eigen/Eigen
DESTINATION include/gtsam/3rdparty/Eigen
FILES_MATCHING PATTERN "*.h")
install(DIRECTORY Eigen/unsupported/Eigen
DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
FILES_MATCHING PATTERN "*.h")
endif()
option(GTSAM_BUILD_METIS "Build metis library" ON)
option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
if(GTSAM_BUILD_METIS)
add_subdirectory(metis-5.1.0)
endif(GTSAM_BUILD_METIS)
add_subdirectory(metis)
add_subdirectory(ceres)
############ NOTE: When updating GeographicLib be sure to disable building their examples
############ and unit tests by commenting out their lines:
# add_subdirectory (examples)
@ -58,10 +58,10 @@ endif(GTSAM_BUILD_METIS)
include(GeographicLib/cmake/FindGeographicLib.cmake)
# Set up the option to install GeographicLib
if(GEOGRAPHICLIB_FOUND)
set(install_geographiclib_default OFF)
else()
if(GEOGRAPHICLIB-NOTFOUND)
set(install_geographiclib_default ON)
else()
set(install_geographiclib_default OFF)
endif()
option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default})
@ -73,3 +73,5 @@ endif()
if(GTSAM_INSTALL_GEOGRAPHICLIB)
add_subdirectory(GeographicLib)
endif()
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

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

@ -1,4 +1,4 @@
repo: 8a21fd850624c931e448cbcfb38168cb2717c790
node: ffa86ffb557094721ca71dcea6aed2651b9fd610
node: 10219c95fe653d4962aa9db4946f6fbea96dd740
branch: 3.2
tag: 3.2.0
tag: 3.2.4

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

@ -23,3 +23,7 @@ bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2
da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1
4b687cad1d23066f66863f4f87298447298443df 3.2-rc1
1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2
ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1
1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2
36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3

2
gtsam/3rdparty/Eigen/Eigen/src/Cholesky/LDLT.h vendored
View File

@ -442,6 +442,7 @@ LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
m_transpositions.resize(size);
m_isInitialized = false;
m_temporary.resize(size);
m_sign = internal::ZeroSign;
internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign);
@ -502,7 +503,6 @@ struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
using std::abs;
using std::max;
typedef typename LDLTType::MatrixType MatrixType;
typedef typename LDLTType::Scalar Scalar;
typedef typename LDLTType::RealScalar RealScalar;
const typename Diagonal<const MatrixType>::RealReturnType vectorD(dec().vectorD());
// In some previous versions, tolerance was set to the max of 1/highest and the maximal diagonal entry * epsilon

10
gtsam/3rdparty/Eigen/Eigen/src/Core/ArrayWrapper.h vendored
View File

@ -29,6 +29,11 @@ struct traits<ArrayWrapper<ExpressionType> >
: public traits<typename remove_all<typename ExpressionType::Nested>::type >
{
typedef ArrayXpr XprKind;
// Let's remove NestByRefBit
enum {
Flags0 = traits<typename remove_all<typename ExpressionType::Nested>::type >::Flags,
Flags = Flags0 & ~NestByRefBit
};
};
}
@ -149,6 +154,11 @@ struct traits<MatrixWrapper<ExpressionType> >
: public traits<typename remove_all<typename ExpressionType::Nested>::type >
{
typedef MatrixXpr XprKind;
// Let's remove NestByRefBit
enum {
Flags0 = traits<typename remove_all<typename ExpressionType::Nested>::type >::Flags,
Flags = Flags0 & ~NestByRefBit
};
};
}

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

@ -462,8 +462,10 @@ template<typename Derived> class DenseBase
template<int p> RealScalar lpNorm() const;
template<int RowFactor, int ColFactor>
const Replicate<Derived,RowFactor,ColFactor> replicate() const;
const Replicate<Derived,Dynamic,Dynamic> replicate(Index rowFacor,Index colFactor) const;
inline const Replicate<Derived,RowFactor,ColFactor> replicate() const;
typedef Replicate<Derived,Dynamic,Dynamic> ReplicateReturnType;
inline const ReplicateReturnType replicate(Index rowFacor,Index colFactor) const;
typedef Reverse<Derived, BothDirections> ReverseReturnType;
typedef const Reverse<const Derived, BothDirections> ConstReverseReturnType;

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

@ -190,18 +190,18 @@ MatrixBase<Derived>::diagonal() const
*
* \sa MatrixBase::diagonal(), class Diagonal */
template<typename Derived>
inline typename MatrixBase<Derived>::template DiagonalIndexReturnType<DynamicIndex>::Type
inline typename MatrixBase<Derived>::DiagonalDynamicIndexReturnType
MatrixBase<Derived>::diagonal(Index index)
{
return typename DiagonalIndexReturnType<DynamicIndex>::Type(derived(), index);
return DiagonalDynamicIndexReturnType(derived(), index);
}
/** This is the const version of diagonal(Index). */
template<typename Derived>
inline typename MatrixBase<Derived>::template ConstDiagonalIndexReturnType<DynamicIndex>::Type
inline typename MatrixBase<Derived>::ConstDiagonalDynamicIndexReturnType
MatrixBase<Derived>::diagonal(Index index) const
{
return typename ConstDiagonalIndexReturnType<DynamicIndex>::Type(derived(), index);
return ConstDiagonalDynamicIndexReturnType(derived(), index);
}
/** \returns an expression of the \a DiagIndex-th sub or super diagonal of the matrix \c *this

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

@ -232,7 +232,7 @@ EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest&
// FIXME not very good if rhs is real and lhs complex while alpha is real too
const Index cols = dest.cols();
for (Index j=0; j<cols; ++j)
func(dest.col(j), prod.rhs().coeff(j) * prod.lhs());
func(dest.col(j), prod.rhs().coeff(0,j) * prod.lhs());
}
// Row major
@ -243,7 +243,7 @@ EIGEN_DONT_INLINE void outer_product_selector_run(const ProductType& prod, Dest&
// FIXME not very good if lhs is real and rhs complex while alpha is real too
const Index rows = dest.rows();
for (Index i=0; i<rows; ++i)
func(dest.row(i), prod.lhs().coeff(i) * prod.rhs());
func(dest.row(i), prod.lhs().coeff(i,0) * prod.rhs());
}
template<typename Lhs, typename Rhs>

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

@ -168,6 +168,7 @@ template<typename Derived> class MapBase<Derived, ReadOnlyAccessors>
template<typename Derived> class MapBase<Derived, WriteAccessors>
: public MapBase<Derived, ReadOnlyAccessors>
{
typedef MapBase<Derived, ReadOnlyAccessors> ReadOnlyMapBase;
public:
typedef MapBase<Derived, ReadOnlyAccessors> Base;
@ -230,11 +231,13 @@ template<typename Derived> class MapBase<Derived, WriteAccessors>
Derived& operator=(const MapBase& other)
{
Base::Base::operator=(other);
ReadOnlyMapBase::Base::operator=(other);
return derived();
}
using Base::Base::operator=;
// In theory we could simply refer to Base:Base::operator=, but MSVC does not like Base::Base,
// see bugs 821 and 920.
using ReadOnlyMapBase::Base::operator=;
};
#undef EIGEN_STATIC_ASSERT_INDEX_BASED_ACCESS

16
gtsam/3rdparty/Eigen/Eigen/src/Core/MatrixBase.h vendored
View File

@ -215,7 +215,7 @@ template<typename Derived> class MatrixBase
typedef Diagonal<Derived> DiagonalReturnType;
DiagonalReturnType diagonal();
typedef typename internal::add_const<Diagonal<const Derived> >::type ConstDiagonalReturnType;
typedef typename internal::add_const<Diagonal<const Derived> >::type ConstDiagonalReturnType;
ConstDiagonalReturnType diagonal() const;
template<int Index> struct DiagonalIndexReturnType { typedef Diagonal<Derived,Index> Type; };
@ -223,16 +223,12 @@ template<typename Derived> class MatrixBase
template<int Index> typename DiagonalIndexReturnType<Index>::Type diagonal();
template<int Index> typename ConstDiagonalIndexReturnType<Index>::Type diagonal() const;
typedef Diagonal<Derived,DynamicIndex> DiagonalDynamicIndexReturnType;
typedef typename internal::add_const<Diagonal<const Derived,DynamicIndex> >::type ConstDiagonalDynamicIndexReturnType;
// Note: The "MatrixBase::" prefixes are added to help MSVC9 to match these declarations with the later implementations.
// On the other hand they confuse MSVC8...
#if (defined _MSC_VER) && (_MSC_VER >= 1500) // 2008 or later
typename MatrixBase::template DiagonalIndexReturnType<DynamicIndex>::Type diagonal(Index index);
typename MatrixBase::template ConstDiagonalIndexReturnType<DynamicIndex>::Type diagonal(Index index) const;
#else
typename DiagonalIndexReturnType<DynamicIndex>::Type diagonal(Index index);
typename ConstDiagonalIndexReturnType<DynamicIndex>::Type diagonal(Index index) const;
#endif
DiagonalDynamicIndexReturnType diagonal(Index index);
ConstDiagonalDynamicIndexReturnType diagonal(Index index) const;
#ifdef EIGEN2_SUPPORT
template<unsigned int Mode> typename internal::eigen2_part_return_type<Derived, Mode>::type part();

5
gtsam/3rdparty/Eigen/Eigen/src/Core/PermutationMatrix.h vendored
View File

@ -555,7 +555,10 @@ struct permut_matrix_product_retval
const Index n = Side==OnTheLeft ? rows() : cols();
// FIXME we need an is_same for expression that is not sensitive to constness. For instance
// is_same_xpr<Block<const Matrix>, Block<Matrix> >::value should be true.
if(is_same<MatrixTypeNestedCleaned,Dest>::value && extract_data(dst) == extract_data(m_matrix))
if( is_same<MatrixTypeNestedCleaned,Dest>::value
&& blas_traits<MatrixTypeNestedCleaned>::HasUsableDirectAccess
&& blas_traits<Dest>::HasUsableDirectAccess
&& extract_data(dst) == extract_data(m_matrix))
{
// apply the permutation inplace
Matrix<bool,PermutationType::RowsAtCompileTime,1,0,PermutationType::MaxRowsAtCompileTime> mask(m_permutation.size());

14
gtsam/3rdparty/Eigen/Eigen/src/Core/ProductBase.h vendored
View File

@ -85,7 +85,14 @@ class ProductBase : public MatrixBase<Derived>
public:
#ifndef EIGEN_NO_MALLOC
typedef typename Base::PlainObject BasePlainObject;
typedef Matrix<Scalar,RowsAtCompileTime==1?1:Dynamic,ColsAtCompileTime==1?1:Dynamic,BasePlainObject::Options> DynPlainObject;
typedef typename internal::conditional<(BasePlainObject::SizeAtCompileTime==Dynamic) || (BasePlainObject::SizeAtCompileTime*int(sizeof(Scalar)) < int(EIGEN_STACK_ALLOCATION_LIMIT)),
BasePlainObject, DynPlainObject>::type PlainObject;
#else
typedef typename Base::PlainObject PlainObject;
#endif
ProductBase(const Lhs& a_lhs, const Rhs& a_rhs)
: m_lhs(a_lhs), m_rhs(a_rhs)
@ -180,7 +187,12 @@ namespace internal {
template<typename Lhs, typename Rhs, int Mode, int N, typename PlainObject>
struct nested<GeneralProduct<Lhs,Rhs,Mode>, N, PlainObject>
{
typedef PlainObject const& type;
typedef typename GeneralProduct<Lhs,Rhs,Mode>::PlainObject const& type;
};
template<typename Lhs, typename Rhs, int Mode, int N, typename PlainObject>
struct nested<const GeneralProduct<Lhs,Rhs,Mode>, N, PlainObject>
{
typedef typename GeneralProduct<Lhs,Rhs,Mode>::PlainObject const& type;
};
}

15
gtsam/3rdparty/Eigen/Eigen/src/Core/Ref.h vendored
View File

@ -188,6 +188,8 @@ template<typename PlainObjectType, int Options, typename StrideType> class Ref
: public RefBase<Ref<PlainObjectType, Options, StrideType> >
{
typedef internal::traits<Ref> Traits;
template<typename Derived>
inline Ref(const PlainObjectBase<Derived>& expr);
public:
typedef RefBase<Ref> Base;
@ -196,20 +198,21 @@ template<typename PlainObjectType, int Options, typename StrideType> class Ref
#ifndef EIGEN_PARSED_BY_DOXYGEN
template<typename Derived>
inline Ref(PlainObjectBase<Derived>& expr,
typename internal::enable_if<bool(Traits::template match<Derived>::MatchAtCompileTime),Derived>::type* = 0)
inline Ref(PlainObjectBase<Derived>& expr)
{
Base::construct(expr);
EIGEN_STATIC_ASSERT(static_cast<bool>(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
Base::construct(expr.derived());
}
template<typename Derived>
inline Ref(const DenseBase<Derived>& expr,
typename internal::enable_if<bool(internal::is_lvalue<Derived>::value&&bool(Traits::template match<Derived>::MatchAtCompileTime)),Derived>::type* = 0,
int = Derived::ThisConstantIsPrivateInPlainObjectBase)
inline Ref(const DenseBase<Derived>& expr)
#else
template<typename Derived>
inline Ref(DenseBase<Derived>& expr)
#endif
{
EIGEN_STATIC_ASSERT(static_cast<bool>(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
EIGEN_STATIC_ASSERT(static_cast<bool>(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
enum { THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY = Derived::ThisConstantIsPrivateInPlainObjectBase};
Base::construct(expr.const_cast_derived());
}

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

@ -135,7 +135,7 @@ template<typename MatrixType,int RowFactor,int ColFactor> class Replicate
*/
template<typename Derived>
template<int RowFactor, int ColFactor>
inline const Replicate<Derived,RowFactor,ColFactor>
const Replicate<Derived,RowFactor,ColFactor>
DenseBase<Derived>::replicate() const
{
return Replicate<Derived,RowFactor,ColFactor>(derived());
@ -150,7 +150,7 @@ DenseBase<Derived>::replicate() const
* \sa VectorwiseOp::replicate(), DenseBase::replicate<int,int>(), class Replicate
*/
template<typename Derived>
inline const Replicate<Derived,Dynamic,Dynamic>
const typename DenseBase<Derived>::ReplicateReturnType
DenseBase<Derived>::replicate(Index rowFactor,Index colFactor) const
{
return Replicate<Derived,Dynamic,Dynamic>(derived(),rowFactor,colFactor);

21
gtsam/3rdparty/Eigen/Eigen/src/Core/TriangularMatrix.h vendored
View File

@ -380,19 +380,19 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularView
EIGEN_STRONG_INLINE TriangularView& operator=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
{
setZero();
return assignProduct(other,1);
return assignProduct(other.derived(),1);
}
template<typename ProductDerived, typename Lhs, typename Rhs>
EIGEN_STRONG_INLINE TriangularView& operator+=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
{
return assignProduct(other,1);
return assignProduct(other.derived(),1);
}
template<typename ProductDerived, typename Lhs, typename Rhs>
EIGEN_STRONG_INLINE TriangularView& operator-=(const ProductBase<ProductDerived, Lhs,Rhs>& other)
{
return assignProduct(other,-1);
return assignProduct(other.derived(),-1);
}
@ -400,25 +400,34 @@ template<typename _MatrixType, unsigned int _Mode> class TriangularView
EIGEN_STRONG_INLINE TriangularView& operator=(const ScaledProduct<ProductDerived>& other)
{
setZero();
return assignProduct(other,other.alpha());
return assignProduct(other.derived(),other.alpha());
}
template<typename ProductDerived>
EIGEN_STRONG_INLINE TriangularView& operator+=(const ScaledProduct<ProductDerived>& other)
{
return assignProduct(other,other.alpha());
return assignProduct(other.derived(),other.alpha());
}
template<typename ProductDerived>
EIGEN_STRONG_INLINE TriangularView& operator-=(const ScaledProduct<ProductDerived>& other)
{
return assignProduct(other,-other.alpha());
return assignProduct(other.derived(),-other.alpha());
}
protected:
template<typename ProductDerived, typename Lhs, typename Rhs>
EIGEN_STRONG_INLINE TriangularView& assignProduct(const ProductBase<ProductDerived, Lhs,Rhs>& prod, const Scalar& alpha);
template<int Mode, bool LhsIsTriangular,
typename Lhs, bool LhsIsVector,
typename Rhs, bool RhsIsVector>
EIGEN_STRONG_INLINE TriangularView& assignProduct(const TriangularProduct<Mode, LhsIsTriangular, Lhs, LhsIsVector, Rhs, RhsIsVector>& prod, const Scalar& alpha)
{
lazyAssign(alpha*prod.eval());
return *this;
}
MatrixTypeNested m_matrix;
};

2
gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/Complex.h vendored
View File

@ -110,7 +110,7 @@ template<> EIGEN_STRONG_INLINE Packet2cf ploaddup<Packet2cf>(const std::complex<
template<> EIGEN_STRONG_INLINE void pstore <std::complex<float> >(std::complex<float> * to, const Packet2cf& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((float*)to, from.v); }
template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float> * to, const Packet2cf& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((float*)to, from.v); }
template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> * addr) { __pld((float *)addr); }
template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> * addr) { EIGEN_ARM_PREFETCH((float *)addr); }
template<> EIGEN_STRONG_INLINE std::complex<float> pfirst<Packet2cf>(const Packet2cf& a)
{

19
gtsam/3rdparty/Eigen/Eigen/src/Core/arch/NEON/PacketMath.h vendored
View File

@ -48,9 +48,18 @@ typedef uint32x4_t Packet4ui;
#define EIGEN_INIT_NEON_PACKET2(X, Y) {X, Y}
#define EIGEN_INIT_NEON_PACKET4(X, Y, Z, W) {X, Y, Z, W}
#endif
#ifndef __pld
#define __pld(x) asm volatile ( " pld [%[addr]]\n" :: [addr] "r" (x) : "cc" );
// arm64 does have the pld instruction. If available, let's trust the __builtin_prefetch built-in function
// which available on LLVM and GCC (at least)
#if EIGEN_HAS_BUILTIN(__builtin_prefetch) || defined(__GNUC__)
#define EIGEN_ARM_PREFETCH(ADDR) __builtin_prefetch(ADDR);
#elif defined __pld
#define EIGEN_ARM_PREFETCH(ADDR) __pld(ADDR)
#elif !defined(__aarch64__)
#define EIGEN_ARM_PREFETCH(ADDR) __asm__ __volatile__ ( " pld [%[addr]]\n" :: [addr] "r" (ADDR) : "cc" );
#else
// by default no explicit prefetching
#define EIGEN_ARM_PREFETCH(ADDR)
#endif
template<> struct packet_traits<float> : default_packet_traits
@ -209,8 +218,8 @@ template<> EIGEN_STRONG_INLINE void pstore<int>(int* to, const Packet4i& f
template<> EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to, from); }
template<> EIGEN_STRONG_INLINE void pstoreu<int>(int* to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to, from); }
template<> EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) { __pld(addr); }
template<> EIGEN_STRONG_INLINE void prefetch<int>(const int* addr) { __pld(addr); }
template<> EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) { EIGEN_ARM_PREFETCH(addr); }
template<> EIGEN_STRONG_INLINE void prefetch<int>(const int* addr) { EIGEN_ARM_PREFETCH(addr); }
// FIXME only store the 2 first elements ?
template<> EIGEN_STRONG_INLINE float pfirst<Packet4f>(const Packet4f& a) { float EIGEN_ALIGN16 x[4]; vst1q_f32(x, a); return x[0]; }

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

@ -52,7 +52,7 @@ Packet4f plog<Packet4f>(const Packet4f& _x)
Packet4i emm0;
Packet4f invalid_mask = _mm_cmplt_ps(x, _mm_setzero_ps());
Packet4f invalid_mask = _mm_cmpnge_ps(x, _mm_setzero_ps()); // not greater equal is true if x is NaN
Packet4f iszero_mask = _mm_cmpeq_ps(x, _mm_setzero_ps());
x = pmax(x, p4f_min_norm_pos); /* cut off denormalized stuff */
@ -166,7 +166,7 @@ Packet4f pexp<Packet4f>(const Packet4f& _x)
emm0 = _mm_cvttps_epi32(fx);
emm0 = _mm_add_epi32(emm0, p4i_0x7f);
emm0 = _mm_slli_epi32(emm0, 23);
return pmul(y, _mm_castsi128_ps(emm0));
return pmax(pmul(y, Packet4f(_mm_castsi128_ps(emm0))), _x);
}
template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
Packet2d pexp<Packet2d>(const Packet2d& _x)
@ -239,7 +239,7 @@ Packet2d pexp<Packet2d>(const Packet2d& _x)
emm0 = _mm_add_epi32(emm0, p4i_1023_0);
emm0 = _mm_slli_epi32(emm0, 20);
emm0 = _mm_shuffle_epi32(emm0, _MM_SHUFFLE(1,2,0,3));
return pmul(x, _mm_castsi128_pd(emm0));
return pmax(pmul(x, Packet2d(_mm_castsi128_pd(emm0))), _x);
}
/* evaluation of 4 sines at onces, using SSE2 intrinsics.

10
gtsam/3rdparty/Eigen/Eigen/src/Core/products/CoeffBasedProduct.h vendored
View File

@ -90,6 +90,7 @@ struct traits<CoeffBasedProduct<LhsNested,RhsNested,NestingFlags> >
| (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0),
CoeffReadCost = InnerSize == Dynamic ? Dynamic
: InnerSize == 0 ? 0
: InnerSize * (NumTraits<Scalar>::MulCost + LhsCoeffReadCost + RhsCoeffReadCost)
+ (InnerSize - 1) * NumTraits<Scalar>::AddCost,
@ -133,7 +134,7 @@ class CoeffBasedProduct
};
typedef internal::product_coeff_impl<CanVectorizeInner ? InnerVectorizedTraversal : DefaultTraversal,
Unroll ? InnerSize-1 : Dynamic,
Unroll ? (InnerSize==0 ? 0 : InnerSize-1) : Dynamic,
_LhsNested, _RhsNested, Scalar> ScalarCoeffImpl;
typedef CoeffBasedProduct<LhsNested,RhsNested,NestByRefBit> LazyCoeffBasedProductType;
@ -184,7 +185,7 @@ class CoeffBasedProduct
{
PacketScalar res;
internal::product_packet_impl<Flags&RowMajorBit ? RowMajor : ColMajor,
Unroll ? InnerSize-1 : Dynamic,
Unroll ? (InnerSize==0 ? 0 : InnerSize-1) : Dynamic,
_LhsNested, _RhsNested, PacketScalar, LoadMode>
::run(row, col, m_lhs, m_rhs, res);
return res;
@ -262,10 +263,7 @@ struct product_coeff_impl<DefaultTraversal, Dynamic, Lhs, Rhs, RetScalar>
typedef typename Lhs::Index Index;
static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, RetScalar& res)
{
eigen_assert(lhs.cols()>0 && "you are using a non initialized matrix");
res = lhs.coeff(row, 0) * rhs.coeff(0, col);
for(Index i = 1; i < lhs.cols(); ++i)
res += lhs.coeff(row, i) * rhs.coeff(i, col);
res = (lhs.row(row).transpose().cwiseProduct( rhs.col(col) )).sum();
}
};

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

@ -13,7 +13,7 @@
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 2
#define EIGEN_MINOR_VERSION 2
#define EIGEN_MINOR_VERSION 4
#define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
(EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
@ -96,6 +96,13 @@
#define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t
#endif
// Cross compiler wrapper around LLVM's __has_builtin
#ifdef __has_builtin
# define EIGEN_HAS_BUILTIN(x) __has_builtin(x)
#else
# define EIGEN_HAS_BUILTIN(x) 0
#endif
/** Allows to disable some optimizations which might affect the accuracy of the result.
* Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them.
* They currently include:
@ -247,7 +254,7 @@ namespace Eigen {
#if !defined(EIGEN_ASM_COMMENT)
#if (defined __GNUC__) && ( defined(__i386__) || defined(__x86_64__) )
#define EIGEN_ASM_COMMENT(X) asm("#" X)
#define EIGEN_ASM_COMMENT(X) __asm__("#" X)
#else
#define EIGEN_ASM_COMMENT(X)
#endif
@ -306,7 +313,7 @@ namespace Eigen {
// just an empty macro !
#define EIGEN_EMPTY
#if defined(_MSC_VER) && (!defined(__INTEL_COMPILER))
#if defined(_MSC_VER) && (_MSC_VER < 1900) && (!defined(__INTEL_COMPILER))
#define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
using Base::operator =;
#elif defined(__clang__) // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653)

10
gtsam/3rdparty/Eigen/Eigen/src/Core/util/Memory.h vendored
View File

@ -63,7 +63,7 @@
// Currently, let's include it only on unix systems:
#if defined(__unix__) || defined(__unix)
#include <unistd.h>
#if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0)
#if ((defined __QNXNTO__) || (defined _GNU_SOURCE) || (defined __PGI) || ((defined _XOPEN_SOURCE) && (_XOPEN_SOURCE >= 600))) && (defined _POSIX_ADVISORY_INFO) && (_POSIX_ADVISORY_INFO > 0)
#define EIGEN_HAS_POSIX_MEMALIGN 1
#endif
#endif
@ -417,6 +417,8 @@ template<typename T, bool Align> inline T* conditional_aligned_realloc_new(T* pt
template<typename T, bool Align> inline T* conditional_aligned_new_auto(size_t size)
{
if(size==0)
return 0; // short-cut. Also fixes Bug 884
check_size_for_overflow<T>(size);
T *result = reinterpret_cast<T*>(conditional_aligned_malloc<Align>(sizeof(T)*size));
if(NumTraits<T>::RequireInitialization)
@ -464,9 +466,8 @@ template<typename T, bool Align> inline void conditional_aligned_delete_auto(T *
template<typename Scalar, typename Index>
static inline Index first_aligned(const Scalar* array, Index size)
{
enum { PacketSize = packet_traits<Scalar>::size,
PacketAlignedMask = PacketSize-1
};
static const Index PacketSize = packet_traits<Scalar>::size;
static const Index PacketAlignedMask = PacketSize-1;
if(PacketSize==1)
{
@ -612,7 +613,6 @@ template<typename T> class aligned_stack_memory_handler
void* operator new(size_t size, const std::nothrow_t&) throw() { \
try { return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); } \
catch (...) { return 0; } \
return 0; \
}
#else
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \

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

@ -90,7 +90,9 @@
YOU_PASSED_A_COLUMN_VECTOR_BUT_A_ROW_VECTOR_WAS_EXPECTED,
THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE,
THE_STORAGE_ORDER_OF_BOTH_SIDES_MUST_MATCH,
OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG
OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG,
IMPLICIT_CONVERSION_TO_SCALAR_IS_FOR_INNER_PRODUCT_ONLY,
STORAGE_LAYOUT_DOES_NOT_MATCH
};
};

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

@ -341,7 +341,7 @@ template<typename T, int n=1, typename PlainObject = typename eval<T>::type> str
};
template<typename T>
T* const_cast_ptr(const T* ptr)
inline T* const_cast_ptr(const T* ptr)
{
return const_cast<T*>(ptr);
}

1
gtsam/3rdparty/Eigen/Eigen/src/Eigen2Support/LeastSquares.h vendored
View File

@ -147,7 +147,6 @@ void fitHyperplane(int numPoints,
// compute the covariance matrix
CovMatrixType covMat = CovMatrixType::Zero(size, size);
VectorType remean = VectorType::Zero(size);
for(int i = 0; i < numPoints; ++i)
{
VectorType diff = (*(points[i]) - mean).conjugate();

2
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/RealQZ.h vendored
View File

@ -313,7 +313,7 @@ namespace Eigen {
using std::abs;
using std::sqrt;
const Index dim=m_S.cols();
if (abs(m_S.coeff(i+1,i)==Scalar(0)))
if (abs(m_S.coeff(i+1,i))==Scalar(0))
return;
Index z = findSmallDiagEntry(i,i+1);
if (z==i-1)

18
gtsam/3rdparty/Eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h vendored
View File

@ -563,7 +563,6 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
if(computeEigenvectors)
{
Scalar safeNorm2 = Eigen::NumTraits<Scalar>::epsilon();
safeNorm2 *= safeNorm2;
if((eivals(2)-eivals(0))<=Eigen::NumTraits<Scalar>::epsilon())
{
eivecs.setIdentity();
@ -577,7 +576,7 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
Scalar d0 = eivals(2) - eivals(1);
Scalar d1 = eivals(1) - eivals(0);
int k = d0 > d1 ? 2 : 0;
d0 = d0 > d1 ? d1 : d0;
d0 = d0 > d1 ? d0 : d1;
tmp.diagonal().array () -= eivals(k);
VectorType cross;
@ -585,19 +584,25 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
n = (cross = tmp.row(0).cross(tmp.row(1))).squaredNorm();
if(n>safeNorm2)
{
eivecs.col(k) = cross / sqrt(n);
}
else
{
n = (cross = tmp.row(0).cross(tmp.row(2))).squaredNorm();
if(n>safeNorm2)
{
eivecs.col(k) = cross / sqrt(n);
}
else
{
n = (cross = tmp.row(1).cross(tmp.row(2))).squaredNorm();
if(n>safeNorm2)
{
eivecs.col(k) = cross / sqrt(n);
}
else
{
// the input matrix and/or the eigenvaues probably contains some inf/NaN,
@ -617,12 +622,16 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
tmp.diagonal().array() -= eivals(1);
if(d0<=Eigen::NumTraits<Scalar>::epsilon())
{
eivecs.col(1) = eivecs.col(k).unitOrthogonal();
}
else
{
n = (cross = eivecs.col(k).cross(tmp.row(0).normalized())).squaredNorm();
n = (cross = eivecs.col(k).cross(tmp.row(0))).squaredNorm();
if(n>safeNorm2)
{
eivecs.col(1) = cross / sqrt(n);
}
else
{
n = (cross = eivecs.col(k).cross(tmp.row(1))).squaredNorm();
@ -636,13 +645,14 @@ template<typename SolverType> struct direct_selfadjoint_eigenvalues<SolverType,3
else
{
// we should never reach this point,
// if so the last two eigenvalues are likely to ve very closed to each other
// if so the last two eigenvalues are likely to be very close to each other
eivecs.col(1) = eivecs.col(k).unitOrthogonal();
}
}
}
// make sure that eivecs[1] is orthogonal to eivecs[2]
// FIXME: this step should not be needed
Scalar d = eivecs.col(1).dot(eivecs.col(k));
eivecs.col(1) = (eivecs.col(1) - d * eivecs.col(k)).normalized();
}

12
gtsam/3rdparty/Eigen/Eigen/src/Geometry/Hyperplane.h vendored
View File

@ -100,7 +100,17 @@ public:
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(VectorType, 3)
Hyperplane result(p0.size());
result.normal() = (p2 - p0).cross(p1 - p0).normalized();
VectorType v0(p2 - p0), v1(p1 - p0);
result.normal() = v0.cross(v1);
RealScalar norm = result.normal().norm();
if(norm <= v0.norm() * v1.norm() * NumTraits<RealScalar>::epsilon())
{
Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose();
JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV);
result.normal() = svd.matrixV().col(2);
}
else
result.normal() /= norm;
result.offset() = -p0.dot(result.normal());
return result;
}

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

@ -60,6 +60,9 @@ public:
/** Construct a 2D counter clock wise rotation from the angle \a a in radian. */
inline Rotation2D(const Scalar& a) : m_angle(a) {}
/** Default constructor wihtout initialization. The represented rotation is undefined. */
Rotation2D() {}
/** \returns the rotation angle */
inline Scalar angle() const { return m_angle; }
@ -81,10 +84,10 @@ public:
/** Applies the rotation to a 2D vector */
Vector2 operator* (const Vector2& vec) const
{ return toRotationMatrix() * vec; }
template<typename Derived>
Rotation2D& fromRotationMatrix(const MatrixBase<Derived>& m);
Matrix2 toRotationMatrix(void) const;
Matrix2 toRotationMatrix() const;
/** \returns the spherical interpolation between \c *this and \a other using
* parameter \a t. It is in fact equivalent to a linear interpolation.

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

@ -62,6 +62,8 @@ struct transform_construct_from_matrix;
template<typename TransformType> struct transform_take_affine_part;
template<int Mode> struct transform_make_affine;
} // end namespace internal
/** \geometry_module \ingroup Geometry_Module
@ -230,8 +232,7 @@ public:
inline Transform()
{
check_template_params();
if (int(Mode)==Affine)
makeAffine();
internal::transform_make_affine<(int(Mode)==Affine) ? Affine : AffineCompact>::run(m_matrix);
}
inline Transform(const Transform& other)
@ -591,11 +592,7 @@ public:
*/
void makeAffine()
{
if(int(Mode)!=int(AffineCompact))
{
matrix().template block<1,Dim>(Dim,0).setZero();
matrix().coeffRef(Dim,Dim) = Scalar(1);
}
internal::transform_make_affine<int(Mode)>::run(m_matrix);
}
/** \internal
@ -1079,6 +1076,24 @@ Transform<Scalar,Dim,Mode,Options>::fromPositionOrientationScale(const MatrixBas
namespace internal {
template<int Mode>
struct transform_make_affine
{
template<typename MatrixType>
static void run(MatrixType &mat)
{
static const int Dim = MatrixType::ColsAtCompileTime-1;
mat.template block<1,Dim>(Dim,0).setZero();
mat.coeffRef(Dim,Dim) = typename MatrixType::Scalar(1);
}
};
template<>
struct transform_make_affine<AffineCompact>
{
template<typename MatrixType> static void run(MatrixType &) { }
};
// selector needed to avoid taking the inverse of a 3x4 matrix
template<typename TransformType, int Mode=TransformType::Mode>
struct projective_transform_inverse

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

@ -39,7 +39,6 @@ bool bicgstab(const MatrixType& mat, const Rhs& rhs, Dest& x,
int maxIters = iters;
int n = mat.cols();
x = precond.solve(x);
VectorType r = rhs - mat * x;
VectorType r0 = r;
@ -143,7 +142,7 @@ struct traits<BiCGSTAB<_MatrixType,_Preconditioner> >
* SparseMatrix<double> A(n,n);
* // fill A and b
* BiCGSTAB<SparseMatrix<double> > solver;
* solver(A);
* solver.compute(A);
* x = solver.solve(b);
* std::cout << "#iterations: " << solver.iterations() << std::endl;
* std::cout << "estimated error: " << solver.error() << std::endl;

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

@ -219,7 +219,7 @@ class PardisoImpl
void pardisoInit(int type)
{
m_type = type;
bool symmetric = abs(m_type) < 10;
bool symmetric = std::abs(m_type) < 10;
m_iparm[0] = 1; // No solver default
m_iparm[1] = 3; // use Metis for the ordering
m_iparm[2] = 1; // Numbers of processors, value of OMP_NUM_THREADS

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

@ -762,6 +762,7 @@ template<typename _MatrixType, int QRPreconditioner> class JacobiSVD
internal::qr_preconditioner_impl<MatrixType, QRPreconditioner, internal::PreconditionIfMoreColsThanRows> m_qr_precond_morecols;
internal::qr_preconditioner_impl<MatrixType, QRPreconditioner, internal::PreconditionIfMoreRowsThanCols> m_qr_precond_morerows;
MatrixType m_scaledMatrix;
};
template<typename MatrixType, int QRPreconditioner>
@ -808,8 +809,9 @@ void JacobiSVD<MatrixType, QRPreconditioner>::allocate(Index rows, Index cols, u
: 0);
m_workMatrix.resize(m_diagSize, m_diagSize);
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_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);
}
template<typename MatrixType, int QRPreconditioner>
@ -826,21 +828,26 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
// limit for very small denormal numbers to be considered zero in order to avoid infinite loops (see bug 286)
const RealScalar considerAsZero = RealScalar(2) * std::numeric_limits<RealScalar>::denorm_min();
// Scaling factor to reduce over/under-flows
RealScalar scale = matrix.cwiseAbs().maxCoeff();
if(scale==RealScalar(0)) scale = RealScalar(1);
/*** step 1. The R-SVD step: we use a QR decomposition to reduce to the case of a square matrix */
if(!m_qr_precond_morecols.run(*this, matrix) && !m_qr_precond_morerows.run(*this, matrix))
if(m_rows!=m_cols)
{
m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize);
m_scaledMatrix = matrix / scale;
m_qr_precond_morecols.run(*this, m_scaledMatrix);
m_qr_precond_morerows.run(*this, m_scaledMatrix);
}
else
{
m_workMatrix = matrix.block(0,0,m_diagSize,m_diagSize) / scale;
if(m_computeFullU) m_matrixU.setIdentity(m_rows,m_rows);
if(m_computeThinU) m_matrixU.setIdentity(m_rows,m_diagSize);
if(m_computeFullV) m_matrixV.setIdentity(m_cols,m_cols);
if(m_computeThinV) m_matrixV.setIdentity(m_cols, m_diagSize);
}
// Scaling factor to reduce over/under-flows
RealScalar scale = m_workMatrix.cwiseAbs().maxCoeff();
if(scale==RealScalar(0)) scale = RealScalar(1);
m_workMatrix /= scale;
/*** step 2. The main Jacobi SVD iteration. ***/
@ -861,7 +868,8 @@ JacobiSVD<MatrixType, QRPreconditioner>::compute(const MatrixType& matrix, unsig
using std::max;
RealScalar threshold = (max)(considerAsZero, precision * (max)(abs(m_workMatrix.coeff(p,p)),
abs(m_workMatrix.coeff(q,q))));
if((max)(abs(m_workMatrix.coeff(p,q)),abs(m_workMatrix.coeff(q,p))) > threshold)
// We compare both values to threshold instead of calling max to be robust to NaN (See bug 791)
if(abs(m_workMatrix.coeff(p,q))>threshold || abs(m_workMatrix.coeff(q,p)) > threshold)
{
finished = false;

4
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/AmbiVector.h vendored
View File

@ -69,7 +69,7 @@ class AmbiVector
delete[] m_buffer;
if (size<1000)
{
Index allocSize = (size * sizeof(ListEl))/sizeof(Scalar);
Index allocSize = (size * sizeof(ListEl) + sizeof(Scalar) - 1)/sizeof(Scalar);
m_allocatedElements = (allocSize*sizeof(Scalar))/sizeof(ListEl);
m_buffer = new Scalar[allocSize];
}
@ -88,7 +88,7 @@ class AmbiVector
Index copyElements = m_allocatedElements;
m_allocatedElements = (std::min)(Index(m_allocatedElements*1.5),m_size);
Index allocSize = m_allocatedElements * sizeof(ListEl);
allocSize = allocSize/sizeof(Scalar) + (allocSize%sizeof(Scalar)>0?1:0);
allocSize = (allocSize + sizeof(Scalar) - 1)/sizeof(Scalar);
Scalar* newBuffer = new Scalar[allocSize];
memcpy(newBuffer, m_buffer, copyElements * sizeof(ListEl));
delete[] m_buffer;

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

@ -68,6 +68,8 @@ public:
const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
private:
Index nonZeros() const;
};
@ -82,6 +84,7 @@ class BlockImpl<SparseMatrix<_Scalar, _Options, _Index>,BlockRows,BlockCols,true
typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType;
typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _MatrixTypeNested;
typedef Block<SparseMatrixType, BlockRows, BlockCols, true> BlockType;
typedef Block<const SparseMatrixType, BlockRows, BlockCols, true> ConstBlockType;
public:
enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
@ -245,6 +248,93 @@ public:
};
template<typename _Scalar, int _Options, typename _Index, int BlockRows, int BlockCols>
class BlockImpl<const SparseMatrix<_Scalar, _Options, _Index>,BlockRows,BlockCols,true,Sparse>
: public SparseMatrixBase<Block<const SparseMatrix<_Scalar, _Options, _Index>,BlockRows,BlockCols,true> >
{
typedef SparseMatrix<_Scalar, _Options, _Index> SparseMatrixType;
typedef typename internal::remove_all<typename SparseMatrixType::Nested>::type _MatrixTypeNested;
typedef Block<const SparseMatrixType, BlockRows, BlockCols, true> BlockType;
public:
enum { IsRowMajor = internal::traits<BlockType>::IsRowMajor };
EIGEN_SPARSE_PUBLIC_INTERFACE(BlockType)
protected:
enum { OuterSize = IsRowMajor ? BlockRows : BlockCols };
public:
class InnerIterator: public SparseMatrixType::InnerIterator
{
public:
inline InnerIterator(const BlockType& xpr, Index outer)
: SparseMatrixType::InnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer)
{}
inline Index row() const { return IsRowMajor ? m_outer : this->index(); }
inline Index col() const { return IsRowMajor ? this->index() : m_outer; }
protected:
Index m_outer;
};
class ReverseInnerIterator: public SparseMatrixType::ReverseInnerIterator
{
public:
inline ReverseInnerIterator(const BlockType& xpr, Index outer)
: SparseMatrixType::ReverseInnerIterator(xpr.m_matrix, xpr.m_outerStart + outer), m_outer(outer)
{}
inline Index row() const { return IsRowMajor ? m_outer : this->index(); }
inline Index col() const { return IsRowMajor ? this->index() : m_outer; }
protected:
Index m_outer;
};
inline BlockImpl(const SparseMatrixType& xpr, int i)
: m_matrix(xpr), m_outerStart(i), m_outerSize(OuterSize)
{}
inline BlockImpl(const SparseMatrixType& xpr, int startRow, int startCol, int blockRows, int blockCols)
: m_matrix(xpr), m_outerStart(IsRowMajor ? startRow : startCol), m_outerSize(IsRowMajor ? blockRows : blockCols)
{}
inline const Scalar* valuePtr() const
{ return m_matrix.valuePtr() + m_matrix.outerIndexPtr()[m_outerStart]; }
inline const Index* innerIndexPtr() const
{ return m_matrix.innerIndexPtr() + m_matrix.outerIndexPtr()[m_outerStart]; }
inline const Index* outerIndexPtr() const
{ return m_matrix.outerIndexPtr() + m_outerStart; }
Index nonZeros() const
{
if(m_matrix.isCompressed())
return std::size_t(m_matrix.outerIndexPtr()[m_outerStart+m_outerSize.value()])
- std::size_t(m_matrix.outerIndexPtr()[m_outerStart]);
else if(m_outerSize.value()==0)
return 0;
else
return Map<const Matrix<Index,OuterSize,1> >(m_matrix.innerNonZeroPtr()+m_outerStart, m_outerSize.value()).sum();
}
const Scalar& lastCoeff() const
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY(BlockImpl);
eigen_assert(nonZeros()>0);
if(m_matrix.isCompressed())
return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart+1]-1];
else
return m_matrix.valuePtr()[m_matrix.outerIndexPtr()[m_outerStart]+m_matrix.innerNonZeroPtr()[m_outerStart]-1];
}
EIGEN_STRONG_INLINE Index rows() const { return IsRowMajor ? m_outerSize.value() : m_matrix.rows(); }
EIGEN_STRONG_INLINE Index cols() const { return IsRowMajor ? m_matrix.cols() : m_outerSize.value(); }
protected:
typename SparseMatrixType::Nested m_matrix;
Index m_outerStart;
const internal::variable_if_dynamic<Index, OuterSize> m_outerSize;
};
//----------
/** \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this

9
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparseDenseProduct.h vendored
View File

@ -306,15 +306,6 @@ class DenseTimeSparseProduct
DenseTimeSparseProduct& operator=(const DenseTimeSparseProduct&);
};
// sparse * dense
template<typename Derived>
template<typename OtherDerived>
inline const typename SparseDenseProductReturnType<Derived,OtherDerived>::Type
SparseMatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const
{
return typename SparseDenseProductReturnType<Derived,OtherDerived>::Type(derived(), other.derived());
}
} // end namespace Eigen
#endif // EIGEN_SPARSEDENSEPRODUCT_H

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

@ -358,7 +358,8 @@ template<typename Derived> class SparseMatrixBase : public EigenBase<Derived>
/** sparse * dense (returns a dense object unless it is an outer product) */
template<typename OtherDerived>
const typename SparseDenseProductReturnType<Derived,OtherDerived>::Type
operator*(const MatrixBase<OtherDerived> &other) const;
operator*(const MatrixBase<OtherDerived> &other) const
{ return typename SparseDenseProductReturnType<Derived,OtherDerived>::Type(derived(), other.derived()); }
/** \returns an expression of P H P^-1 where H is the matrix represented by \c *this */
SparseSymmetricPermutationProduct<Derived,Upper|Lower> twistedBy(const PermutationMatrix<Dynamic,Dynamic,Index>& perm) const

2
gtsam/3rdparty/Eigen/Eigen/src/SparseCore/SparsePermutation.h vendored
View File

@ -61,7 +61,7 @@ struct permut_sparsematrix_product_retval
for(Index j=0; j<m_matrix.outerSize(); ++j)
{
Index jp = m_permutation.indices().coeff(j);
sizes[((Side==OnTheLeft) ^ Transposed) ? jp : j] = m_matrix.innerVector(((Side==OnTheRight) ^ Transposed) ? jp : j).size();
sizes[((Side==OnTheLeft) ^ Transposed) ? jp : j] = m_matrix.innerVector(((Side==OnTheRight) ^ Transposed) ? jp : j).nonZeros();
}
tmp.reserve(sizes);
for(Index j=0; j<m_matrix.outerSize(); ++j)

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

@ -260,14 +260,13 @@ class SparseLU : public internal::SparseLUImpl<typename _MatrixType::Scalar, typ
eigen_assert(m_factorizationIsOk && "The matrix should be factorized first.");
// Initialize with the determinant of the row matrix
Scalar det = Scalar(1.);
//Note that the diagonal blocks of U are stored in supernodes,
// Note that the diagonal blocks of U are stored in supernodes,
// which are available in the L part :)
for (Index j = 0; j < this->cols(); ++j)
{
for (typename SCMatrix::InnerIterator it(m_Lstore, j); it; ++it)
{
if(it.row() < j) continue;
if(it.row() == j)
if(it.index() == j)
{
det *= (std::abs)(it.value());
break;

4
gtsam/3rdparty/Eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h vendored
View File

@ -189,8 +189,8 @@ class MappedSuperNodalMatrix<Scalar,Index>::InnerIterator
m_idval(mat.colIndexPtr()[outer]),
m_startidval(m_idval),
m_endidval(mat.colIndexPtr()[outer+1]),
m_idrow(mat.rowIndexPtr()[outer]),
m_endidrow(mat.rowIndexPtr()[outer+1])
m_idrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]]),
m_endidrow(mat.rowIndexPtr()[mat.supToCol()[mat.colToSup()[outer]]+1])
{}
inline InnerIterator& operator++()
{

63
gtsam/3rdparty/Eigen/Eigen/src/SparseQR/SparseQR.h vendored
View File

@ -75,7 +75,7 @@ class SparseQR
typedef Matrix<Scalar, Dynamic, 1> ScalarVector;
typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
public:
SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false)
SparseQR () : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
{ }
/** Construct a QR factorization of the matrix \a mat.
@ -84,7 +84,7 @@ class SparseQR
*
* \sa compute()
*/
SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false)
SparseQR(const MatrixType& mat) : m_isInitialized(false), m_analysisIsok(false), m_lastError(""), m_useDefaultThreshold(true),m_isQSorted(false),m_isEtreeOk(false)
{
compute(mat);
}
@ -262,6 +262,7 @@ class SparseQR
IndexVector m_etree; // Column elimination tree
IndexVector m_firstRowElt; // First element in each row
bool m_isQSorted; // whether Q is sorted or not
bool m_isEtreeOk; // whether the elimination tree match the initial input matrix
template <typename, typename > friend struct SparseQR_QProduct;
template <typename > friend struct SparseQRMatrixQReturnType;
@ -281,9 +282,11 @@ template <typename MatrixType, typename OrderingType>
void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
{
eigen_assert(mat.isCompressed() && "SparseQR requires a sparse matrix in compressed mode. Call .makeCompressed() before passing it to SparseQR");
// Copy to a column major matrix if the input is rowmajor
typename internal::conditional<MatrixType::IsRowMajor,QRMatrixType,const MatrixType&>::type matCpy(mat);
// Compute the column fill reducing ordering
OrderingType ord;
ord(mat, m_perm_c);
ord(matCpy, m_perm_c);
Index n = mat.cols();
Index m = mat.rows();
Index diagSize = (std::min)(m,n);
@ -296,7 +299,8 @@ void SparseQR<MatrixType,OrderingType>::analyzePattern(const MatrixType& mat)
// Compute the column elimination tree of the permuted matrix
m_outputPerm_c = m_perm_c.inverse();
internal::coletree(mat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
internal::coletree(matCpy, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
m_isEtreeOk = true;
m_R.resize(m, n);
m_Q.resize(m, diagSize);
@ -330,15 +334,38 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
Index nzcolR, nzcolQ; // Number of nonzero for the current column of R and Q
ScalarVector tval(m); // The dense vector used to compute the current column
RealScalar pivotThreshold = m_threshold;
m_R.setZero();
m_Q.setZero();
m_pmat = mat;
m_pmat.uncompress(); // To have the innerNonZeroPtr allocated
// Apply the fill-in reducing permutation lazily:
for (int i = 0; i < n; i++)
if(!m_isEtreeOk)
{
Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i;
m_pmat.outerIndexPtr()[p] = mat.outerIndexPtr()[i];
m_pmat.innerNonZeroPtr()[p] = mat.outerIndexPtr()[i+1] - mat.outerIndexPtr()[i];
m_outputPerm_c = m_perm_c.inverse();
internal::coletree(m_pmat, m_etree, m_firstRowElt, m_outputPerm_c.indices().data());
m_isEtreeOk = true;
}
m_pmat.uncompress(); // To have the innerNonZeroPtr allocated
// Apply the fill-in reducing permutation lazily:
{
// If the input is row major, copy the original column indices,
// otherwise directly use the input matrix
//
IndexVector originalOuterIndicesCpy;
const Index *originalOuterIndices = mat.outerIndexPtr();
if(MatrixType::IsRowMajor)
{
originalOuterIndicesCpy = IndexVector::Map(m_pmat.outerIndexPtr(),n+1);
originalOuterIndices = originalOuterIndicesCpy.data();
}
for (int i = 0; i < n; i++)
{
Index p = m_perm_c.size() ? m_perm_c.indices()(i) : i;
m_pmat.outerIndexPtr()[p] = originalOuterIndices[i];
m_pmat.innerNonZeroPtr()[p] = originalOuterIndices[i+1] - originalOuterIndices[i];
}
}
/* Compute the default threshold as in MatLab, see:
@ -349,6 +376,8 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
{
RealScalar max2Norm = 0.0;
for (int j = 0; j < n; j++) max2Norm = (max)(max2Norm, m_pmat.col(j).norm());
if(max2Norm==RealScalar(0))
max2Norm = RealScalar(1);
pivotThreshold = 20 * (m + n) * max2Norm * NumTraits<RealScalar>::epsilon();
}
@ -357,7 +386,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
Index nonzeroCol = 0; // Record the number of valid pivots
m_Q.startVec(0);
// Left looking rank-revealing QR factorization: compute a column of R and Q at a time
for (Index col = 0; col < n; ++col)
{
@ -373,7 +402,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
// all the nodes (with indexes lower than rank) reachable through the column elimination tree (etree) rooted at node k.
// Note: if the diagonal entry does not exist, then its contribution must be explicitly added,
// thus the trick with found_diag that permits to do one more iteration on the diagonal element if this one has not been found.
for (typename MatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
for (typename QRMatrixType::InnerIterator itp(m_pmat, col); itp || !found_diag; ++itp)
{
Index curIdx = nonzeroCol;
if(itp) curIdx = itp.row();
@ -447,7 +476,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
}
} // End update current column
Scalar tau;
Scalar tau = 0;
RealScalar beta = 0;
if(nonzeroCol < diagSize)
@ -461,7 +490,6 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
for (Index itq = 1; itq < nzcolQ; ++itq) sqrNorm += numext::abs2(tval(Qidx(itq)));
if(sqrNorm == RealScalar(0) && numext::imag(c0) == RealScalar(0))
{
tau = RealScalar(0);
beta = numext::real(c0);
tval(Qidx(0)) = 1;
}
@ -514,6 +542,7 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
// Recompute the column elimination tree
internal::coletree(m_pmat, m_etree, m_firstRowElt, m_pivotperm.indices().data());
m_isEtreeOk = false;
}
}
@ -525,13 +554,13 @@ void SparseQR<MatrixType,OrderingType>::factorize(const MatrixType& mat)
m_R.finalize();
m_R.makeCompressed();
m_isQSorted = false;
m_nonzeropivots = nonzeroCol;
if(nonzeroCol<n)
{
// Permute the triangular factor to put the 'dead' columns to the end
MatrixType tempR(m_R);
QRMatrixType tempR(m_R);
m_R = tempR * m_pivotperm;
// Update the column permutation

112
gtsam/3rdparty/Eigen/Eigen/src/UmfPackSupport/UmfPackSupport.h vendored
View File

@ -107,6 +107,16 @@ inline int umfpack_get_determinant(std::complex<double> *Mx, double *Ex, void *N
return umfpack_zi_get_determinant(&mx_real,0,Ex,NumericHandle,User_Info);
}
namespace internal {
template<typename T> struct umfpack_helper_is_sparse_plain : false_type {};
template<typename Scalar, int Options, typename StorageIndex>
struct umfpack_helper_is_sparse_plain<SparseMatrix<Scalar,Options,StorageIndex> >
: true_type {};
template<typename Scalar, int Options, typename StorageIndex>
struct umfpack_helper_is_sparse_plain<MappedSparseMatrix<Scalar,Options,StorageIndex> >
: true_type {};
}
/** \ingroup UmfPackSupport_Module
* \brief A sparse LU factorization and solver based on UmfPack
*
@ -192,10 +202,14 @@ class UmfPackLU : internal::noncopyable
* Note that the matrix should be column-major, and in compressed format for best performance.
* \sa SparseMatrix::makeCompressed().
*/
void compute(const MatrixType& matrix)
template<typename InputMatrixType>
void compute(const InputMatrixType& matrix)
{
analyzePattern(matrix);
factorize(matrix);
if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar());
if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar());
grapInput(matrix.derived());
analyzePattern_impl();
factorize_impl();
}
/** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A.
@ -230,23 +244,15 @@ class UmfPackLU : internal::noncopyable
*
* \sa factorize(), compute()
*/
void analyzePattern(const MatrixType& matrix)
template<typename InputMatrixType>
void analyzePattern(const InputMatrixType& matrix)
{
if(m_symbolic)
umfpack_free_symbolic(&m_symbolic,Scalar());
if(m_numeric)
umfpack_free_numeric(&m_numeric,Scalar());
if(m_symbolic) umfpack_free_symbolic(&m_symbolic,Scalar());
if(m_numeric) umfpack_free_numeric(&m_numeric,Scalar());
grapInput(matrix);
grapInput(matrix.derived());
int errorCode = 0;
errorCode = umfpack_symbolic(matrix.rows(), matrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr,
&m_symbolic, 0, 0);
m_isInitialized = true;
m_info = errorCode ? InvalidInput : Success;
m_analysisIsOk = true;
m_factorizationIsOk = false;
analyzePattern_impl();
}
/** Performs a numeric decomposition of \a matrix
@ -255,20 +261,16 @@ class UmfPackLU : internal::noncopyable
*
* \sa analyzePattern(), compute()
*/
void factorize(const MatrixType& matrix)
template<typename InputMatrixType>
void factorize(const InputMatrixType& matrix)
{
eigen_assert(m_analysisIsOk && "UmfPackLU: you must first call analyzePattern()");
if(m_numeric)
umfpack_free_numeric(&m_numeric,Scalar());
grapInput(matrix);
int errorCode;
errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr,
m_symbolic, &m_numeric, 0, 0);
m_info = errorCode ? NumericalIssue : Success;
m_factorizationIsOk = true;
grapInput(matrix.derived());
factorize_impl();
}
#ifndef EIGEN_PARSED_BY_DOXYGEN
@ -283,19 +285,20 @@ class UmfPackLU : internal::noncopyable
protected:
void init()
{
m_info = InvalidInput;
m_isInitialized = false;
m_numeric = 0;
m_symbolic = 0;
m_outerIndexPtr = 0;
m_innerIndexPtr = 0;
m_valuePtr = 0;
m_info = InvalidInput;
m_isInitialized = false;
m_numeric = 0;
m_symbolic = 0;
m_outerIndexPtr = 0;
m_innerIndexPtr = 0;
m_valuePtr = 0;
m_extractedDataAreDirty = true;
}
void grapInput(const MatrixType& mat)
template<typename InputMatrixType>
void grapInput_impl(const InputMatrixType& mat, internal::true_type)
{
m_copyMatrix.resize(mat.rows(), mat.cols());
if( ((MatrixType::Flags&RowMajorBit)==RowMajorBit) || sizeof(typename MatrixType::Index)!=sizeof(int) || !mat.isCompressed() )
@ -313,6 +316,45 @@ class UmfPackLU : internal::noncopyable
m_valuePtr = mat.valuePtr();
}
}
template<typename InputMatrixType>
void grapInput_impl(const InputMatrixType& mat, internal::false_type)
{
m_copyMatrix = mat;
m_outerIndexPtr = m_copyMatrix.outerIndexPtr();
m_innerIndexPtr = m_copyMatrix.innerIndexPtr();
m_valuePtr = m_copyMatrix.valuePtr();
}
template<typename InputMatrixType>
void grapInput(const InputMatrixType& mat)
{
grapInput_impl(mat, internal::umfpack_helper_is_sparse_plain<InputMatrixType>());
}
void analyzePattern_impl()
{
int errorCode = 0;
errorCode = umfpack_symbolic(m_copyMatrix.rows(), m_copyMatrix.cols(), m_outerIndexPtr, m_innerIndexPtr, m_valuePtr,
&m_symbolic, 0, 0);
m_isInitialized = true;
m_info = errorCode ? InvalidInput : Success;
m_analysisIsOk = true;
m_factorizationIsOk = false;
m_extractedDataAreDirty = true;
}
void factorize_impl()
{
int errorCode;
errorCode = umfpack_numeric(m_outerIndexPtr, m_innerIndexPtr, m_valuePtr,
m_symbolic, &m_numeric, 0, 0);
m_info = errorCode ? NumericalIssue : Success;
m_factorizationIsOk = true;
m_extractedDataAreDirty = true;
}
// cached data to reduce reallocation, etc.
mutable LUMatrixType m_l;

18
gtsam/3rdparty/Eigen/cmake/EigenTesting.cmake vendored
View File

@ -452,20 +452,12 @@ macro(ei_set_build_string)
endmacro(ei_set_build_string)
macro(ei_is_64bit_env VAR)
file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp"
"int main() { return (sizeof(int*) == 8 ? 1 : 0); }
")
try_run(run_res compile_res
${CMAKE_CURRENT_BINARY_DIR} "${CMAKE_CURRENT_BINARY_DIR}/is64.cpp"
RUN_OUTPUT_VARIABLE run_output)
if(compile_res AND run_res)
set(${VAR} ${run_res})
elseif(CMAKE_CL_64)
set(${VAR} 1)
elseif("$ENV{Platform}" STREQUAL "X64") # nmake 64 bit
if(CMAKE_SIZEOF_VOID_P EQUAL 8)
set(${VAR} 1)
elseif(CMAKE_SIZEOF_VOID_P EQUAL 4)
set(${VAR} 0)
else()
message(WARNING "Unsupported pointer size. Please contact the authors.")
endif()
endmacro(ei_is_64bit_env)

2
gtsam/3rdparty/Eigen/cmake/FindCholmod.cmake vendored
View File

@ -86,4 +86,4 @@ include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(CHOLMOD DEFAULT_MSG
CHOLMOD_INCLUDES CHOLMOD_LIBRARIES)
mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY)
mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY CAMD_LIBRARY CCOLAMD_LIBRARY CHOLMOD_METIS_LIBRARY)

2
gtsam/3rdparty/Eigen/cmake/FindFFTW.cmake vendored
View File

@ -115,5 +115,5 @@ include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(FFTW DEFAULT_MSG
FFTW_INCLUDES FFTW_LIBRARIES)
mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES)
mark_as_advanced(FFTW_INCLUDES FFTW_LIBRARIES FFTW_LIB FFTWF_LIB FFTWL_LIB)

38
gtsam/3rdparty/Eigen/cmake/FindMetis.cmake vendored
View File

@ -10,16 +10,50 @@ find_path(METIS_INCLUDES
PATHS
$ENV{METISDIR}
${INCLUDE_INSTALL_DIR}
PATH_SUFFIXES
PATH_SUFFIXES
.
metis
include
)
macro(_metis_check_version)
file(READ "${METIS_INCLUDES}/metis.h" _metis_version_header)
string(REGEX MATCH "define[ \t]+METIS_VER_MAJOR[ \t]+([0-9]+)" _metis_major_version_match "${_metis_version_header}")
set(METIS_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+METIS_VER_MINOR[ \t]+([0-9]+)" _metis_minor_version_match "${_metis_version_header}")
set(METIS_MINOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+METIS_VER_SUBMINOR[ \t]+([0-9]+)" _metis_subminor_version_match "${_metis_version_header}")
set(METIS_SUBMINOR_VERSION "${CMAKE_MATCH_1}")
if(NOT METIS_MAJOR_VERSION)
message(WARNING "Could not determine Metis version. Assuming version 4.0.0")
set(METIS_VERSION 4.0.0)
else()
set(METIS_VERSION ${METIS_MAJOR_VERSION}.${METIS_MINOR_VERSION}.${METIS_SUBMINOR_VERSION})
endif()
if(${METIS_VERSION} VERSION_LESS ${Metis_FIND_VERSION})
set(METIS_VERSION_OK FALSE)
else()
set(METIS_VERSION_OK TRUE)
endif()
if(NOT METIS_VERSION_OK)
message(STATUS "Metis version ${METIS_VERSION} found in ${METIS_INCLUDES}, "
"but at least version ${Metis_FIND_VERSION} is required")
endif(NOT METIS_VERSION_OK)
endmacro(_metis_check_version)
if(METIS_INCLUDES AND Metis_FIND_VERSION)
_metis_check_version()
else()
set(METIS_VERSION_OK TRUE)
endif()
find_library(METIS_LIBRARIES metis PATHS $ENV{METISDIR} ${LIB_INSTALL_DIR} PATH_SUFFIXES lib)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(METIS DEFAULT_MSG
METIS_INCLUDES METIS_LIBRARIES)
METIS_INCLUDES METIS_LIBRARIES METIS_VERSION_OK)
mark_as_advanced(METIS_INCLUDES METIS_LIBRARIES)

2
gtsam/3rdparty/Eigen/cmake/language_support.cmake vendored
View File

@ -33,7 +33,7 @@ function(workaround_9220 language language_works)
file(WRITE ${CMAKE_BINARY_DIR}/language_tests/${language}/CMakeLists.txt
${text})
execute_process(
COMMAND ${CMAKE_COMMAND} .
COMMAND ${CMAKE_COMMAND} . -G "${CMAKE_GENERATOR}"
WORKING_DIRECTORY ${CMAKE_BINARY_DIR}/language_tests/${language}
RESULT_VARIABLE return_code
OUTPUT_QUIET

8
gtsam/3rdparty/Eigen/doc/AsciiQuickReference.txt vendored
View File

@ -67,10 +67,10 @@ P.rightCols<cols>() // P(:, end-cols+1:end)
P.rightCols(cols) // P(:, end-cols+1:end)
P.topRows<rows>() // P(1:rows, :)
P.topRows(rows) // P(1:rows, :)
P.middleRows<rows>(i) // P(:, i+1:i+rows)
P.middleRows(i, rows) // P(:, i+1:i+rows)
P.bottomRows<rows>() // P(:, end-rows+1:end)
P.bottomRows(rows) // P(:, end-rows+1:end)
P.middleRows<rows>(i) // P(i+1:i+rows, :)
P.middleRows(i, rows) // P(i+1:i+rows, :)
P.bottomRows<rows>() // P(end-rows+1:end, :)
P.bottomRows(rows) // P(end-rows+1:end, :)
P.topLeftCorner(rows, cols) // P(1:rows, 1:cols)
P.topRightCorner(rows, cols) // P(1:rows, end-cols+1:end)
P.bottomLeftCorner(rows, cols) // P(end-rows+1:end, 1:cols)

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

@ -71,11 +71,10 @@ i.e either row major or column major. The default is column major. Most arithmet
<td> Constant or Random Insertion</td>
<td>
\code
sm1.setZero(); // Set the matrix with zero elements
sm1.setConstant(val); //Replace all the nonzero values with val
sm1.setZero();
\endcode
</td>
<td> The matrix sm1 should have been created before ???</td>
<td>Remove all non-zero coefficients</td>
</tr>
</table>

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

@ -6,12 +6,10 @@ foreach(example_src ${examples_SRCS})
if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO)
target_link_libraries(${example} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO})
endif()
get_target_property(example_executable
${example} LOCATION)
add_custom_command(
TARGET ${example}
POST_BUILD
COMMAND ${example_executable}
COMMAND ${example}
ARGS >${CMAKE_CURRENT_BINARY_DIR}/${example}.out
)
add_dependencies(all_examples ${example})

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

@ -14,12 +14,10 @@ foreach(snippet_src ${snippets_SRCS})
if(EIGEN_STANDARD_LIBRARIES_TO_LINK_TO)
target_link_libraries(${compile_snippet_target} ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO})
endif()
get_target_property(compile_snippet_executable
${compile_snippet_target} LOCATION)
add_custom_command(
TARGET ${compile_snippet_target}
POST_BUILD
COMMAND ${compile_snippet_executable}
COMMAND ${compile_snippet_target}
ARGS >${CMAKE_CURRENT_BINARY_DIR}/${snippet}.out
)
add_dependencies(all_snippets ${compile_snippet_target})
@ -27,4 +25,4 @@ foreach(snippet_src ${snippets_SRCS})
PROPERTIES OBJECT_DEPENDS ${snippet_src})
endforeach(snippet_src)
ei_add_target_property(compile_tut_arithmetic_transpose_aliasing COMPILE_FLAGS -DEIGEN_NO_DEBUG)
ei_add_target_property(compile_tut_arithmetic_transpose_aliasing COMPILE_FLAGS -DEIGEN_NO_DEBUG)

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

@ -1,4 +1,3 @@
if(NOT EIGEN_TEST_NOQT)
find_package(Qt4)
if(QT4_FOUND)
@ -6,16 +5,16 @@ if(NOT EIGEN_TEST_NOQT)
endif()
endif(NOT EIGEN_TEST_NOQT)
if(QT4_FOUND)
add_executable(Tutorial_sparse_example Tutorial_sparse_example.cpp Tutorial_sparse_example_details.cpp)
target_link_libraries(Tutorial_sparse_example ${EIGEN_STANDARD_LIBRARIES_TO_LINK_TO} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY})
add_custom_command(
TARGET Tutorial_sparse_example
POST_BUILD
COMMAND Tutorial_sparse_example ARGS ${CMAKE_CURRENT_BINARY_DIR}/../html/Tutorial_sparse_example.jpeg
)
add_dependencies(all_examples Tutorial_sparse_example)
endif(QT4_FOUND)

6
gtsam/3rdparty/Eigen/failtest/CMakeLists.txt vendored
View File

@ -26,6 +26,12 @@ ei_add_failtest("block_on_const_type_actually_const_1")
ei_add_failtest("transpose_on_const_type_actually_const")
ei_add_failtest("diagonal_on_const_type_actually_const")
ei_add_failtest("ref_1")
ei_add_failtest("ref_2")
ei_add_failtest("ref_3")
ei_add_failtest("ref_4")
ei_add_failtest("ref_5")
if (EIGEN_FAILTEST_FAILURE_COUNT)
message(FATAL_ERROR
"${EIGEN_FAILTEST_FAILURE_COUNT} out of ${EIGEN_FAILTEST_COUNT} failtests FAILED. "

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

@ -66,7 +66,7 @@ endif()
find_package(Pastix)
find_package(Scotch)
find_package(Metis)
find_package(Metis 5.0 REQUIRED)
if(PASTIX_FOUND AND BLAS_FOUND)
add_definitions("-DEIGEN_PASTIX_SUPPORT")
include_directories(${PASTIX_INCLUDES})
@ -279,6 +279,7 @@ ei_add_property(EIGEN_TESTING_SUMMARY "CXX_FLAGS: ${CMAKE_CXX_FLAGS}\n")
ei_add_property(EIGEN_TESTING_SUMMARY "Sparse lib flags: ${SPARSE_LIBS}\n")
option(EIGEN_TEST_EIGEN2 "Run whole Eigen2 test suite against EIGEN2_SUPPORT" OFF)
mark_as_advanced(EIGEN_TEST_EIGEN2)
if(EIGEN_TEST_EIGEN2)
add_subdirectory(eigen2)
endif()

12
gtsam/3rdparty/Eigen/test/cholesky.cpp vendored
View File

@ -320,33 +320,35 @@ template<typename MatrixType> void cholesky_definiteness(const MatrixType& m)
{
eigen_assert(m.rows() == 2 && m.cols() == 2);
MatrixType mat;
LDLT<MatrixType> ldlt(2);
{
mat << 1, 0, 0, -1;
LDLT<MatrixType> ldlt(mat);
ldlt.compute(mat);
VERIFY(!ldlt.isNegative());
VERIFY(!ldlt.isPositive());
}
{
mat << 1, 2, 2, 1;
LDLT<MatrixType> ldlt(mat);
ldlt.compute(mat);
VERIFY(!ldlt.isNegative());
VERIFY(!ldlt.isPositive());
}
{
mat << 0, 0, 0, 0;
LDLT<MatrixType> ldlt(mat);
ldlt.compute(mat);
VERIFY(ldlt.isNegative());
VERIFY(ldlt.isPositive());
}
{
mat << 0, 0, 0, 1;
LDLT<MatrixType> ldlt(mat);
ldlt.compute(mat);
VERIFY(!ldlt.isNegative());
VERIFY(ldlt.isPositive());
}
{
mat << -1, 0, 0, 0;
LDLT<MatrixType> ldlt(mat);
ldlt.compute(mat);
VERIFY(ldlt.isNegative());
VERIFY(!ldlt.isPositive());
}

2
gtsam/3rdparty/Eigen/test/cwiseop.cpp vendored
View File

@ -9,6 +9,8 @@
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
#define EIGEN2_SUPPORT
#define EIGEN_NO_EIGEN2_DEPRECATED_WARNING
#define EIGEN_NO_STATIC_ASSERT
#include "main.h"
#include <functional>

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

@ -87,32 +87,6 @@ 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
@ -128,12 +102,6 @@ void test_dynalloc()
CALL_SUBTEST(check_dynaligned<Matrix4f>() );
CALL_SUBTEST(check_dynaligned<Vector4d>() );
CALL_SUBTEST(check_dynaligned<Vector4i>() );
CALL_SUBTEST(check_dynaligned<Vector8f>() );
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 ?

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

@ -4,6 +4,7 @@ add_dependencies(eigen2_check eigen2_buildtests)
add_dependencies(buildtests eigen2_buildtests)
add_definitions("-DEIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API")
add_definitions("-DEIGEN_NO_EIGEN2_DEPRECATED_WARNING")
ei_add_test(eigen2_meta)
ei_add_test(eigen2_sizeof)

2
gtsam/3rdparty/Eigen/test/eigen2/eigen2_adjoint.cpp vendored
View File

@ -29,8 +29,6 @@ template<typename MatrixType> void adjoint(const MatrixType& m)
MatrixType m1 = MatrixType::Random(rows, cols),
m2 = MatrixType::Random(rows, cols),
m3(rows, cols),
mzero = MatrixType::Zero(rows, cols),
identity = SquareMatrixType::Identity(rows, rows),
square = SquareMatrixType::Random(rows, rows);
VectorType v1 = VectorType::Random(rows),
v2 = VectorType::Random(rows),

3
gtsam/3rdparty/Eigen/test/eigen2/eigen2_basicstuff.cpp vendored
View File

@ -23,11 +23,8 @@ template<typename MatrixType> void basicStuff(const MatrixType& m)
m2 = MatrixType::Random(rows, cols),
m3(rows, cols),
mzero = MatrixType::Zero(rows, cols),
identity = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
::Identity(rows, rows),
square = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>::Random(rows, rows);
VectorType v1 = VectorType::Random(rows),
v2 = VectorType::Random(rows),
vzero = VectorType::Zero(rows);
Scalar x = ei_random<Scalar>();

7
gtsam/3rdparty/Eigen/test/eigen2/eigen2_cwiseop.cpp vendored
View File

@ -35,11 +35,8 @@ template<typename MatrixType> void cwiseops(const MatrixType& m)
mzero = MatrixType::Zero(rows, cols),
mones = MatrixType::Ones(rows, cols),
identity = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>
::Identity(rows, rows),
square = Matrix<Scalar, MatrixType::RowsAtCompileTime, MatrixType::RowsAtCompileTime>::Random(rows, rows);
VectorType v1 = VectorType::Random(rows),
v2 = VectorType::Random(rows),
vzero = VectorType::Zero(rows),
::Identity(rows, rows);
VectorType vzero = VectorType::Zero(rows),
vones = VectorType::Ones(rows),
v3(rows);

1
gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry.cpp vendored
View File

@ -392,6 +392,7 @@ template<typename Scalar> void geometry(void)
#define VERIFY_EULER(I,J,K, X,Y,Z) { \
Vector3 ea = m.eulerAngles(I,J,K); \
Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
VERIFY_IS_APPROX(m, m1); \
VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \
}
VERIFY_EULER(0,1,2, X,Y,Z);

1
gtsam/3rdparty/Eigen/test/eigen2/eigen2_geometry_with_eigen2_prefix.cpp vendored
View File

@ -394,6 +394,7 @@ template<typename Scalar> void geometry(void)
#define VERIFY_EULER(I,J,K, X,Y,Z) { \
Vector3 ea = m.eulerAngles(I,J,K); \
Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
VERIFY_IS_APPROX(m, m1); \
VERIFY_IS_APPROX(m, Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \
}
VERIFY_EULER(0,1,2, X,Y,Z);

1
gtsam/3rdparty/Eigen/test/eigen2/eigen2_inverse.cpp vendored
View File

@ -25,7 +25,6 @@ template<typename MatrixType> void inverse(const MatrixType& m)
MatrixType m1 = MatrixType::Random(rows, cols),
m2(rows, cols),
mzero = MatrixType::Zero(rows, cols),
identity = MatrixType::Identity(rows, rows);
while(ei_abs(m1.determinant()) < RealScalar(0.1) && rows <= 8)

3
gtsam/3rdparty/Eigen/test/eigen2/eigen2_linearstructure.cpp vendored
View File

@ -25,8 +25,7 @@ template<typename MatrixType> void linearStructure(const MatrixType& m)
// to test it, hence I consider that we will have tested Random.h
MatrixType m1 = MatrixType::Random(rows, cols),
m2 = MatrixType::Random(rows, cols),
m3(rows, cols),
mzero = MatrixType::Zero(rows, cols);
m3(rows, cols);
Scalar s1 = ei_random<Scalar>();
while (ei_abs(s1)<1e-3) s1 = ei_random<Scalar>();

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