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

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

release/4.3a0
nsrinivasan7 2015-02-11 13:28:51 -05:00
parent 734f0fbdf3 bed39acd4c
commit 1b634b5472
1732 changed files with 773968 additions and 19776 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2660
.cproject
View File

File diff suppressed because it is too large Load Diff

6
.gitignore vendored
View File

@ -1,2 +1,8 @@
/build*
*.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

4
.project
View File

@ -23,7 +23,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value>-j5</value>
<value>-j4</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
@ -63,7 +63,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
<value>false</value>
<value>true</value>
</dictionary>
</arguments>
</buildCommand>

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

106
CMakeLists.txt
View File

@ -2,8 +2,14 @@
project(GTSAM CXX C)
cmake_minimum_required(VERSION 2.6)
# new feature to Cmake Version > 2.8.12
# Mac ONLY. Define Relative Path on Mac OS
if(NOT DEFINED CMAKE_MACOSX_RPATH)
set(CMAKE_MACOSX_RPATH 0)
endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 3)
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 0)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
@ -34,7 +40,7 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
endif()
# See whether gtsam_unstable is available (it will be present only if we're using an SVN checkout)
# See whether gtsam_unstable is available (it will be present only if we're using a git checkout)
if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable")
set(GTSAM_UNSTABLE_AVAILABLE 1)
else()
@ -46,14 +52,10 @@ endif()
# Set up options
# Configurable Options
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" OFF) # These do not currently work
option(GTSAM_BUILD_EXAMPLES "Enable/Disable building of examples" ON)
if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON)
endif()
option(GTSAM_BUILD_SHARED_LIBRARY "Enable/Disable building of a shared version of gtsam" ON)
option(GTSAM_BUILD_STATIC_LIBRARY "Enable/Disable building of a static version of gtsam" OFF)
option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead of shared" OFF)
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)
@ -76,27 +78,14 @@ if(GTSAM_INSTALL_WRAP AND NOT GTSAM_BUILD_WRAP)
message(FATAL_ERROR "GTSAM_INSTALL_WRAP is enabled, please also enable GTSAM_BUILD_WRAP")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.")
endif()
# Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
# Sanity check building of libraries
if (NOT GTSAM_BUILD_SHARED_LIBRARY AND NOT GTSAM_BUILD_STATIC_LIBRARY)
message(FATAL_ERROR "Both shared and static version of GTSAM library disabled - need to choose at least one!")
endif()
# Flags to determine whether tests and examples are build during 'make install'
# Note that these remove the targets from the 'all'
option(GTSAM_DISABLE_TESTS_ON_INSTALL "Disables building tests during install" ON)
option(GTSAM_DISABLE_EXAMPLES_ON_INSTALL "Disables building examples during install" OFF)
# Pull in infrastructure
if (GTSAM_BUILD_TESTS)
enable_testing()
include(Dart)
include(CTest)
endif()
###############################################################################
# Find boost
@ -107,10 +96,10 @@ endif()
# If using Boost shared libs, disable auto linking
if(MSVC)
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols
add_definitions(-DBOOST_ALL_DYN_LINK)
# Disable autolinking
if(NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_NO_LIB)
add_definitions(-DBOOST_ALL_DYN_LINK)
endif()
endif()
@ -134,11 +123,17 @@ else()
if(Boost_TIMER_LIBRARY)
list(APPEND GTSAM_BOOST_LIBRARIES ${Boost_TIMER_LIBRARY} ${Boost_CHRONO_LIBRARY})
else()
list(APPEND GTSAM_BOOST_LIBRARIES rt) # When using the header-only boost timer library, need -lrt
message("WARNING: GTSAM timing instrumentation will use the older, less accurate, Boost timer library because boost older than 1.48 was found.")
endif()
endif()
if(NOT (${Boost_VERSION} LESS 105600))
message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES)
endif()
###############################################################################
# Find TBB
find_package(TBB)
@ -178,17 +173,21 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
include_directories(${MKL_INCLUDE_DIR})
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
else()
set(GTSAM_USE_EIGEN_MKL 0)
set(EIGEN_USE_MKL_ALL 0)
endif()
###############################################################################
# Find OpenMP
find_package(OpenMP)
# Find OpenMP (if we're also using MKL)
find_package(OpenMP) # do this here to generate correct message if disabled
if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP)
set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
endif()
endif()
###############################################################################
@ -223,7 +222,6 @@ configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eig
# Install the configuration file for Eigen
install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty)
###############################################################################
# Global compile options
@ -272,16 +270,23 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR})
include_directories(BEFORE
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.
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
add_definitions(-Wno-unused-local-typedefs)
endif()
endif()
if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
@ -291,19 +296,6 @@ endif()
###############################################################################
# Add components
# Set default library - static or shared, before adding subdirectories
if(GTSAM_BUILD_SHARED_LIBRARY)
set(gtsam-default gtsam-shared)
if(GTSAM_BUILD_UNSTABLE)
set(gtsam_unstable-default gtsam_unstable-shared)
endif()
else()
set(gtsam-default gtsam-static)
if(GTSAM_BUILD_UNSTABLE)
set(gtsam_unstable-default gtsam_unstable-static)
endif()
endif()
# Build CppUnitLite
add_subdirectory(CppUnitLite)
@ -319,9 +311,10 @@ add_subdirectory(gtsam)
add_subdirectory(tests)
# Build examples
if (GTSAM_BUILD_EXAMPLES)
add_subdirectory(examples)
endif(GTSAM_BUILD_EXAMPLES)
# Build timing
add_subdirectory(timing)
# Matlab toolbox
if (GTSAM_INSTALL_MATLAB_TOOLBOX)
@ -337,6 +330,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)
@ -345,13 +339,16 @@ if (DOXYGEN_FOUND)
add_subdirectory(doc)
endif()
# CMake Tools
add_subdirectory(cmake)
###############################################################################
# Set up CPack
set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology")
set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu")
set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README")
set(CPACK_PACKAGE_DESCRIPTION_FILE "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR})
@ -375,24 +372,23 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>=
message(STATUS "===============================================================")
message(STATUS "================ Configuration Options ======================")
message(STATUS "Build flags ")
print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ")
print_config_flag(${GTSAM_BUILD_EXAMPLES} "Build Examples ")
print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ")
print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ")
print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'")
if (DOXYGEN_FOUND)
print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ")
endif()
print_config_flag(${GTSAM_BUILD_SHARED_LIBRARY} "Build shared GTSAM Library ")
print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM Library ")
print_config_flag(${GTSAM_BUILD_STATIC_LIBRARY} "Build static GTSAM library instead of shared")
print_config_flag(${GTSAM_BUILD_TYPE_POSTFIXES} "Put build type in library name ")
if(GTSAM_UNSTABLE_AVAILABLE)
print_config_flag(${GTSAM_BUILD_UNSTABLE} "Build libgtsam_unstable ")
endif()
print_config_flag(${GTSAM_DISABLE_TESTS_ON_INSTALL} "No tests in all or install ")
print_config_flag(${GTSAM_DISABLE_EXAMPLES_ON_INSTALL} "No examples in all or install ")
string(TOUPPER "${CMAKE_BUILD_TYPE}" cmake_build_type_toupper)
if(NOT MSVC AND NOT XCODE_VERSION)
message(STATUS " Build type : ${CMAKE_BUILD_TYPE}")
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_TBB)
message(STATUS " Use Intel TBB : Yes")
elseif(TBB_FOUND)

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

19
DEVELOP Normal file
View File

@ -0,0 +1,19 @@
Information for developers
Coding Conventions:
* Classes are Uppercase, methods and functions lowerMixedCase
* We use a modified K&R Style, with 2-space tabs, inserting spaces for tabs
* Use meaningful variable names, e.g., measurement not msm
Windows:
On Windows it is necessary to explicitly export all functions from the library
which should be externally accessible. To do this, include the macro
GTSAM_EXPORT in your class or function definition.
For example:
class GTSAM_EXPORT MyClass { ... };
GTSAM_EXPORT myFunction();

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.

56
README → INSTALL
View File

@ -1,6 +1,3 @@
README - Georgia Tech Smoothing and Mapping library
---------------------------------------------------
Quickstart
In the root library folder execute:
@ -11,42 +8,6 @@ $] cmake ..
$] make check (optional, runs unit tests)
$] make install
---------------------------------------------------
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.
GTSAM is not (yet) open source: See COPYING & LICENSE
Please see USAGE for an example on how to use GTSAM.
The library is organized according to the following directory structure:
3rdparty local copies of third party libraries - Eigen3 and CCOLAMD
base provides some base Math and data structures, as well as test-related utilities
geometry points, poses, tensors, etc
inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees
linear inference specialized to Gaussian linear case, GaussianFactorGraph etc...
nonlinear non-linear factor graphs and non-linear optimization
slam SLAM and visual SLAM application code
This library contains unchanged copies of two third party libraries, with documentation
of licensing as follows:
- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree ordering library
- http://www.cise.ufl.edu/research/sparse
- Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
- Eigen 3.2: General C++ matrix and linear algebra library
- Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README (some code that is 3rd-party to
Eigen is BSD and LGPL)
There are two supporting libraries:
CppUnitLite unit test library customized for use with gtsam
wrap code generation utility for the Matlab interface to gtsam
Important Installation Notes
----------------------------
@ -63,16 +24,23 @@ Optional dependent libraries:
may be installed from the Ubuntu repositories, and for other platforms it
may be downloaded from https://www.threadingbuildingblocks.org/
Tested compilers
Tested compilers:
- GCC 4.2-4.7
- Clang 2.9-3.2
- OSX Clang 2.9-5.0
- OSX GCC 4.2
- MSVC 2010, 2012
Tested systems:
- Ubuntu 11.04, 11.10, 12.04, 12.10, 13.04
- MacOS 10.6, 10.7
- Windows 7
- Ubuntu 11.04 - 13.10
- MacOS 10.6 - 10.9
- Windows 7, 8, 8.1
Known issues:
- MSVC 2013 is not yet supported because it cannot build the serialization module
of Boost 1.55 (or earlier).
2)
GTSAM makes extensive use of debug assertions, and we highly recommend you work

43
LICENSE
View File

@ -1,29 +1,18 @@
Copyright (c) 2010, Georgia Tech Research Corporation
Atlanta, Georgia 30332-0415
All Rights Reserved
GTSAM is released under the simplified BSD license, reproduced in the file
LICENSE.BSD in this directory.
See also README for licensing of 3rd-party code included in GTSAM.
GTSAM contains two third party libraries, with documentation of licensing and
modifications as follows:
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holders nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree
ordering library
- Included unmodified in gtsam/3rdparty/CCOLAMD and gtsam/3rdparty/UFconfig
- http://www.cise.ufl.edu/research/sparse
- Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
- Eigen 3.2: General C++ matrix and linear algebra library
- Modified with 3 patches that have been contributed back to the Eigen team:
- 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)
- Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README
- Some code that is 3rd-party to Eigen is BSD and LGPL

26
LICENSE.BSD
View File

@ -2,28 +2,12 @@ Copyright (c) 2010, Georgia Tech Research Corporation
Atlanta, Georgia 30332-0415
All Rights Reserved
See also README for licensing of 3rd-party code included in GTSAM.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
* Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
* Neither the name of the copyright holders nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY
EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

64
README.md
View File

@ -1,7 +1,20 @@
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:
@ -24,52 +37,15 @@ 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)
Tested compilers
Additional Information
----------------------
- GCC 4.2-4.7
- OSX Clang 2.9-5.0
- OSX GCC 4.2
- MSVC 2010, 2012
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
Tested systems:
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
- Ubuntu 11.04 - 13.10
- MacOS 10.6 - 10.9
- Windows 7, 8
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
See the `INSTALL` file for more detailed installation instructions.
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.
GTSAM is open source under the BSD license, see the `LICENSE.BSD` file.
Please see the `examples/` directory and the `USAGE` file for examples on how to use GTSAM.
The library is organized according to the following directory structure:
3rdparty local copies of third party libraries - Eigen3 and CCOLAMD
base provides some base Math and data structures, as well as test-related utilities
geometry points, poses, tensors, etc
inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees
linear inference specialized to Gaussian linear case, GaussianFactorGraph etc...
nonlinear non-linear factor graphs and non-linear optimization
slam SLAM and visual SLAM application code
This library contains unchanged copies of two third party libraries, with documentation
of licensing as follows:
- CCOLAMD 2.73: Tim Davis' constrained column approximate minimum degree ordering library
- http://www.cise.ufl.edu/research/sparse
- Licenced under LGPL v2.1, provided in gtsam/3rdparty/CCOLAMD/Doc/lesser.txt
- Eigen 3.2: General C++ matrix and linear algebra library
- Licenced under MPL2, provided in gtsam/3rdparty/Eigen/COPYING.README (some code that is 3rd-party to Eigen is BSD and LGPL)
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

70
USAGE
View File

@ -1,70 +0,0 @@
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,
using a visual SLAM implementation as an example.
Getting Started
---------------------------------------------------
Install:
Follow the installation instructions in the README file to build and
install gtsam, as well as running tests to ensure the library is working
properly.
Compiling/Linking with gtsam:
The installation creates a binary "libgtsam" at the installation prefix,
and an include folder "gtsam". These are the only required includes, but
the library has also been designed to make use of XML serialization through
the Boost.serialization library, which requires the the Boost.serialization
headers and binaries to be linked.
If you use CMake for your project, you can use the CMake scripts in the
cmake folder for finding GTSAM, CppUnitLite, and Wrap.
Examples:
To see how the library works, examine the unit tests provided.
Overview
---------------------------------------------------
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.
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

54
USAGE.md Normal file
View File

@ -0,0 +1,54 @@
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,
using a visual SLAM implementation as an example.
Getting Started
---------------------------------------------------
Install:
Follow the installation instructions in the README file to build and
install gtsam, as well as running tests to ensure the library is working
properly.
Compiling/Linking with gtsam:
The installation creates a binary "libgtsam" at the installation prefix,
and an include folder "gtsam". These are the only required includes, but
the library has also been designed to make use of XML serialization through
the Boost.serialization library, which requires the the Boost.serialization
headers and binaries to be linked.
If you use CMake for your project, you can use the CMake scripts in the
cmake folder for finding GTSAM, CppUnitLite, and Wrap.
Examples:
To see how the library works, examine the unit tests provided.
Overview
---------------------------------------------------
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.
The library is organized according to the following directory structure:
3rdparty local copies of third party libraries - Eigen3 and CCOLAMD
base provides some base Math and data structures, as well as test-related utilities
geometry points, poses, tensors, etc
inference core graphical model inference such as factor graphs, junction trees, Bayes nets, Bayes trees
linear inference specialized to Gaussian linear case, GaussianFactorGraph etc...
nonlinear non-linear factor graphs and non-linear optimization
slam SLAM and visual SLAM application code

26
cmake/CMakeLists.txt Normal file
View File

@ -0,0 +1,26 @@
# This file installs the scripts from this directory that may be used in other
# projects. See README.txt in this directory for documentation.
# Set the install directory depending on the platform so it will be found by
# find_package(GTSAMCMakeTools)
if(WIN32 AND NOT CYGWIN)
set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/CMake")
else()
set(SCRIPT_INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/lib/cmake")
endif()
# Install scripts
install(FILES
GTSAMCMakeToolsConfig.cmake
Config.cmake.in
dllexport.h.in
GtsamBuildTypes.cmake
GtsamMakeConfigFile.cmake
GtsamMatlabWrap.cmake
GtsamPythonWrap.cmake
GtsamTesting.cmake
GtsamTestingObsolete.cmake
README.html
DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")

9
cmake/FindMKL.cmake
View File

@ -58,6 +58,7 @@ FIND_PATH(MKL_ROOT_DIR
/opt/intel/mkl/*/
/opt/intel/cmkl/
/opt/intel/cmkl/*/
/opt/intel/*/mkl/
/Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal
"C:/Program Files (x86)/Intel/ComposerXE-2011/mkl"
"C:/Program Files (x86)/Intel/Composer XE 2013/mkl"
@ -137,12 +138,15 @@ ELSE() # UNIX and macOS
${MKL_ROOT_DIR}/lib/
)
# MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
IF(NOT APPLE)
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
mkl_gnu_thread
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
ENDIF()
# Intel Libraries
IF("${MKL_ARCH_DIR}" STREQUAL "32")
@ -226,7 +230,12 @@ ELSE() # UNIX and macOS
endforeach()
endforeach()
IF(APPLE)
SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES})
ELSE()
SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
ENDIF()
MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY
MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
ENDIF()

4
cmake/GTSAMCMakeToolsConfig.cmake Normal file
View File

@ -0,0 +1,4 @@
# This config file modifies CMAKE_MODULE_PATH so that the GTSAM-CMakeTools files may be included
set(GTSAM_CMAKE_TOOLS_DIR "${CMAKE_CURRENT_LIST_DIR}")
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}")

20
cmake/GtsamBuildTypes.cmake
View File

@ -10,17 +10,13 @@ if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSI
endif()
# Add option for using build type postfixes to allow installing multiple build modes
if(MSVC OR XCODE_VERSION)
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
else()
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" OFF)
endif()
# 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)
@ -38,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)
@ -58,15 +54,15 @@ if(NOT FIRST_PASS_DONE)
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(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
add_definitions(-ftemplate-depth=1024)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
endif()
endif()
# Set up build type library postfixes
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type Debug Timing RelWithDebInfo MinSizeRel)
foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
string(TOUPPER "${build_type}" build_type_toupper)
set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
endforeach()
@ -106,7 +102,7 @@ if( NOT cmake_build_type_tolower STREQUAL ""
endif()
# Mark that first pass is done
set(FIRST_PASS_DONE TRUE CACHE BOOL "Internally used to mark whether cmake has been run multiple times" FORCE)
set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times")
mark_as_advanced(FIRST_PASS_DONE)
# Enable Visual Studio solution folders

248
cmake/GtsamMatlabWrap.cmake
View File

@ -28,31 +28,40 @@ endif()
# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
# on the system path.
list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred
find_program(mex_command ${mex_program_name}
find_program(MEX_COMMAND ${mex_program_name}
PATHS ${matlab_bin_directories} ENV PATH
NO_DEFAULT_PATH)
mark_as_advanced(FORCE mex_command)
mark_as_advanced(FORCE MEX_COMMAND)
# Now that we have mex, trace back to find the Matlab installation root
get_filename_component(mex_command "${mex_command}" REALPATH)
get_filename_component(mex_path "${mex_command}" PATH)
get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
get_filename_component(mex_path "${MEX_COMMAND}" PATH)
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
# User-friendly wrapping function. Builds a mex module from the provided
# interfaceHeader. For example, for the interface header /path/to/gtsam.h,
# interfaceHeader. For example, for the interface header gtsam.h,
# this will build the wrap module 'gtsam'.
# Params:
# interfaceHeader : Absolute or relative path to the interface definition file
# linkLibraries : All dependent CMake target names, library names, or full library paths
# extraIncludeDirs : Extra include directories, in addition to those already passed to include_directories(...)
# extraMexFlags : Any additional compiler flags
#
# Arguments:
#
# interfaceHeader: The relative path to the wrapper interface definition file.
# linkLibraries: Any *additional* libraries to link. Your project library
# (e.g. `lba`), libraries it depends on, and any necessary
# MATLAB libraries will be linked automatically. So normally,
# leave this empty.
# extraIncludeDirs: Any *additional* include paths required by dependent
# libraries that have not already been added by
# include_directories. Again, normally, leave this empty.
# extraMexFlags: Any *additional* flags to pass to the compiler when building
# the wrap code. Normally, leave this empty.
function(wrap_and_install_library interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
wrap_library_internal("${interfaceHeader}" "${otherLibraries}" "${extraIncludeDirs}" "${mexFlags}")
wrap_library_internal("${interfaceHeader}" "${linkLibraries}" "${extraIncludeDirs}" "${mexFlags}")
install_wrapped_library_internal("${interfaceHeader}")
endfunction()
# Internal function that wraps a library and compiles the wrapper
function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)
if(UNIX AND NOT APPLE)
if(CMAKE_SIZEOF_VOID_P EQUAL 8)
@ -84,7 +93,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
# Paths for generated files
set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
set(generated_cpp_file "${PROJECT_BINARY_DIR}/wrap/${moduleName}/${moduleName}_wrapper.cpp")
set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp")
set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex")
message(STATUS "Building wrap module ${moduleName}")
@ -100,29 +109,105 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
set(matlab_h_path "${installed_includes_path}/wrap")
endif()
# Add -shared or -static suffix to targets
set(correctedOtherLibraries "")
set(otherLibraryTargets "")
foreach(lib ${moduleName} ${otherLibraries})
if(TARGET ${lib})
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryTargets ${lib})
elseif(TARGET ${lib}-shared) # Prefer the shared library if we have both shared and static)
list(APPEND correctedOtherLibraries ${lib}-shared)
list(APPEND otherLibraryTargets ${lib}-shared)
elseif(TARGET ${lib}-static)
list(APPEND correctedOtherLibraries ${lib}-static)
list(APPEND otherLibraryTargets ${lib}-static)
else()
list(APPEND correctedOtherLibraries ${lib})
# If building a static mex module, add all cmake-linked libraries to the
# explicit link libraries list so that the next block of code can unpack
# any static libraries
set(automaticDependencies "")
foreach(lib ${moduleName} ${linkLibraries})
#message("MODULE NAME: ${moduleName}")
if(TARGET "${lib}")
get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
# message("DEPENDENT LIBRARIES: ${dependentLibraries}")
if(dependentLibraries)
list(APPEND automaticDependencies ${dependentLibraries})
endif()
endif()
endforeach()
## CHRIS: Temporary fix. On my system the get_target_property above returned Not-found for gtsam module
## This needs to be fixed!!
if(UNIX AND NOT APPLE)
list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE}
${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE}
${Boost_REGEX_LIBRARY_RELEASE})
if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0
list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE})
if(GTSAM_MEX_BUILD_STATIC_MODULE)
#list(APPEND automaticDependencies -Wl,--no-as-needed -lrt)
endif()
endif()
endif()
#message("AUTOMATIC DEPENDENCIES: ${automaticDependencies}")
## CHRIS: End temporary fix
# Separate dependencies
set(correctedOtherLibraries "")
set(otherLibraryTargets "")
set(otherLibraryNontargets "")
set(otherSourcesAndObjects "")
foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
if(TARGET "${lib}")
if(GTSAM_MEX_BUILD_STATIC_MODULE)
get_target_property(target_sources ${lib} SOURCES)
list(APPEND otherSourcesAndObjects ${target_sources})
else()
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryTargets ${lib})
endif()
else()
get_filename_component(file_extension "${lib}" EXT)
get_filename_component(lib_name "${lib}" NAME_WE)
if(file_extension STREQUAL ".a" AND GTSAM_MEX_BUILD_STATIC_MODULE)
# For building a static MEX module, unpack the static library
# and compile its object files into our module
file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects")
execute_process(COMMAND ar -x "${lib}"
WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects"
RESULT_VARIABLE ar_result)
if(NOT ar_result EQUAL 0)
message(FATAL_ERROR "Failed extracting ${lib}")
endif()
# Get list of object files
execute_process(COMMAND ar -t "${lib}"
OUTPUT_VARIABLE object_files
RESULT_VARIABLE ar_result)
if(NOT ar_result EQUAL 0)
message(FATAL_ERROR "Failed listing ${lib}")
endif()
# Add directory to object files
string(REPLACE "\n" ";" object_files_list "${object_files}")
foreach(object_file ${object_files_list})
get_filename_component(file_extension "${object_file}" EXT)
if(file_extension STREQUAL ".o")
list(APPEND otherSourcesAndObjects "${generated_files_path}/${lib_name}_objects/${object_file}")
endif()
endforeach()
else()
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryNontargets ${lib})
endif()
endif()
endforeach()
# Check libraries for conflicting versions built-in to MATLAB
set(dependentLibraries "")
if(NOT "${otherLibraryTargets}" STREQUAL "")
foreach(target ${otherLibraryTargets})
get_target_property(dependentLibrariesOne ${target} INTERFACE_LINK_LIBRARIES)
list(APPEND dependentLibraries ${dependentLibrariesOne})
endforeach()
endif()
list(APPEND dependentLibraries ${otherLibraryNontargets})
check_conflicting_libraries_internal("${dependentLibraries}")
# Set up generation of module source file
file(MAKE_DIRECTORY "${generated_files_path}")
add_custom_command(
OUTPUT ${generated_cpp_file}
DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets}
DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
COMMAND
wrap
${modulePath}
@ -135,7 +220,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
# Set up building of mex module
string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}")
string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
add_library(${moduleName}_wrapper MODULE ${generated_cpp_file} ${interfaceHeader})
add_library(${moduleName}_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects})
target_link_libraries(${moduleName}_wrapper ${correctedOtherLibraries})
set_target_properties(${moduleName}_wrapper PROPERTIES
OUTPUT_NAME "${moduleName}_wrapper"
@ -174,6 +259,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
COMMAND cmake -E remove_directory ${compiled_mex_modules_root})
endfunction()
# Internal function that installs a wrap toolbox
function(install_wrapped_library_internal interfaceHeader)
get_filename_component(moduleName "${interfaceHeader}" NAME_WE)
set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
@ -205,53 +291,77 @@ function(install_wrapped_library_internal interfaceHeader)
endif()
endfunction()
# Function to setup codegen and building of the wrap toolbox
#
# params:
# moduleName : the name of the module, interface file must be called moduleName.h
# mexFlags : Compilation flags to be passed to the mex compiler
# modulePath : relative path to module markup header file (called moduleName.h)
# otherLibraries : list of library targets this should depend on
# toolboxPath : the directory in which to generate/build wrappers
# wrap_header_path : path to the installed wrap header
function(wrap_library_generic moduleName mexFlags modulePath otherLibraries toolbox_path wrap_header_path)
if(NOT "${CMAKE_PROJECT_NAME}" STREQUAL "GTSAM")
message("Your project uses wrap_library or wrap_library_generic - this is deprecated, please use the more user-friendly function wrap_and_install_library")
# Internal function to check for libraries installed with MATLAB that may conflict
# and prints a warning to move them if problems occur.
function(check_conflicting_libraries_internal libraries)
if(UNIX)
# Set path for matlab's built-in libraries
if(APPLE)
set(mxLibPath "${MATLAB_ROOT}/bin/maci64")
else()
if(CMAKE_CL_64)
set(mxLibPath "${MATLAB_ROOT}/bin/glnxa64")
else()
set(mxLibPath "${MATLAB_ROOT}/bin/glnx86")
endif()
endif()
# Append module name to link libraries to keep original behavior
list(APPEND otherLibraries ${moduleName})
# List matlab's built-in libraries
file(GLOB matlabLibs RELATIVE "${mxLibPath}" "${mxLibPath}/lib*")
# Set up arguments
set(interfaceHeader ${modulePath}/${moduleName}.h)
# Convert to base names
set(matlabLibNames "")
foreach(lib ${matlabLibs})
get_filename_component(libName "${lib}" NAME_WE)
list(APPEND matlabLibNames "${libName}")
endforeach()
# Call internal function
wrap_library_internal("${interfaceHeader}" "${otherLibraries}" "" "${mexFlags}")
endfunction(wrap_library_generic)
# Get names of link libraries
set(linkLibNames "")
foreach(lib ${libraries})
string(FIND "${lib}" "/" slashPos)
if(NOT slashPos EQUAL -1)
# If the name is a path, just get the library name
get_filename_component(libName "${lib}" NAME_WE)
list(APPEND linkLibNames "${libName}")
else()
# It's not a path, so see if it looks like a filename
get_filename_component(ext "${lib}" EXT)
if(NOT "${ext}" STREQUAL "")
# It's a filename, so get the base name
get_filename_component(libName "${lib}" NAME_WE)
list(APPEND linkLibNames "${libName}")
else()
# It's not a filename so it must be a short name, add the "lib" prefix
list(APPEND linkLibNames "lib${lib}")
endif()
endif()
endforeach()
# Function to setup codegen, building and installation of the wrap toolbox
# This wrap setup function assumes that the toolbox will be installed directly,
# with predictable matlab.h sourcing. Use this version when the toolbox will be used
# from the installed version, rather than in place.
# Assumes variable GTSAM_WRAP_HEADER_PATH has been set
# params:
# moduleName : the name of the module, interface file must be called moduleName.h
# mexFlags : Compilation flags to be passed to the mex compiler
# modulePath : relative path to module markup header file (called moduleName.h)
# otherLibraries : list of library targets this should depend on
function(wrap_library moduleName mexFlags modulePath otherLibraries)
# Toolbox generation path goes in build folder
set(toolbox_base_path ${PROJECT_BINARY_DIR}/wrap)
set(toolbox_path ${toolbox_base_path}/${moduleName})
# Remove duplicates
list(REMOVE_DUPLICATES linkLibNames)
# Call generic version of function
wrap_library_generic("${moduleName}" "${mexFlags}" "${modulePath}" "${otherLibraries}" "${toolbox_path}" "${GTSAM_WRAP_HEADER_PATH}")
set(conflictingLibs "")
foreach(lib ${linkLibNames})
list(FIND matlabLibNames "${lib}" libPos)
if(NOT libPos EQUAL -1)
if(NOT conflictingLibs STREQUAL "")
set(conflictingLibs "${conflictingLibs}, ")
endif()
set(conflictingLibs "${conflictingLibs}${lib}")
endif()
endforeach()
install_wrapped_library_internal("${modulePath}/${moduleName}.h")
endfunction(wrap_library)
if(NOT "${conflictingLibs}" STREQUAL "")
message(WARNING "GTSAM links to the libraries [ ${conflictingLibs} ] on your system, but "
"MATLAB is distributed with its own versions of these libraries which may conflict. "
"If you get strange errors or crashes with the GTSAM MATLAB wrapper, move these "
"libraries out of MATLAB's built-in library directory, which is ${mxLibPath} on "
"your system. MATLAB will usually still work with these libraries moved away, but "
"if not, you'll have to compile the static GTSAM MATLAB wrapper module.")
endif()
endif()
endfunction()
# Helper function to install MATLAB scripts and handle multiple build types where the scripts
# should be installed to all build type toolboxes

380
cmake/GtsamTesting.cmake
View File

@ -1,111 +1,145 @@
# Build macros for using tests
# This file defines the two macros below for easily adding groups of unit tests and scripts,
# as well as sets up unit testing and defines several cache options used to control how
# tests and scripts are built and run.
###############################################################################
# Macro:
#
# gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries)
#
# Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the
# tests to create. Tests are assigned into a group name so they can easily by run
# independently with a make target. Running 'make check' builds and runs all tests.
#
# Usage example:
# gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib")
#
# Arguments:
# groupName: A name that will allow this group of tests to be run independently, e.g.
# 'basic' causes a 'check.basic' target to be created to run this test
# group.
# globPatterns: The list of files or glob patterns from which to create unit tests, with
# one test created for each cpp file. e.g. "test*.cpp", or
# "testA*.cpp;testB*.cpp;testOneThing.cpp".
# excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp".
# Pass an empty string "" if nothing needs to be excluded.
# linkLibraries: The list of libraries to link to in addition to CppUnitLite.
macro(gtsamAddTestsGlob groupName globPatterns excludedFiles linkLibraries)
gtsamAddTestsGlob_impl("${groupName}" "${globPatterns}" "${excludedFiles}" "${linkLibraries}")
endmacro()
###############################################################################
# Macro:
#
# gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)
#
# Add scripts that will serve as examples of how to use the library. A list of files or
# glob patterns is specified, and one executable will be created for each matching .cpp
# file. These executables will not be installed. They are built with 'make all' if
# GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.
#
# Usage example:
# gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
#
# Arguments:
# globPatterns: The list of files or glob patterns from which to create examples, with
# one program created for each cpp file. e.g. "*.cpp", or
# "A*.cpp;B*.cpp;MyExample.cpp".
# excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
# an empty string "" if nothing needs to be excluded.
# linkLibraries: The list of libraries to link to.
macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries)
gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${GTSAM_BUILD_EXAMPLES_ALWAYS})
endmacro()
###############################################################################
# Macro:
#
# gtsamAddTimingGlob(globPatterns excludedFiles linkLibraries)
#
# Add scripts that time aspects of the library. A list of files or
# glob patterns is specified, and one executable will be created for each matching .cpp
# file. These executables will not be installed. They are not built with 'make all',
# but they may be built with 'make timing'.
#
# Usage example:
# gtsamAddTimingGlob("*.cpp" "DisabledTimingScript.cpp" "gtsam;GeographicLib")
#
# Arguments:
# globPatterns: The list of files or glob patterns from which to create programs, with
# one program created for each cpp file. e.g. "*.cpp", or
# "A*.cpp;B*.cpp;MyExample.cpp".
# excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
# an empty string "" if nothing needs to be excluded.
# linkLibraries: The list of libraries to link to.
macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries)
gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${GTSAM_BUILD_TIMING_ALWAYS})
endmacro()
# Implementation follows:
# Build macros for using tests
enable_testing()
# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
add_custom_target(timing)
option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON)
option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON)
option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF)
# Add option for combining unit tests
if(MSVC)
if(MSVC OR XCODE_VERSION)
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" ON)
else()
option(GTSAM_SINGLE_TEST_EXE "Combine unit tests into single executable (faster compile)" OFF)
endif()
mark_as_advanced(GTSAM_SINGLE_TEST_EXE)
# Macro for adding categorized tests in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with CppUnitLite and boost
# Arguments:
# - subdir The name of the category for this test
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_tests A list of test files that should not be compiled - use for debugging
function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests)
# Subdirectory target for tests
add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
set(is_test TRUE)
# Put check target in Visual Studio solution folder
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}")
# Link with CppUnitLite - pulled from gtsam installation
list(APPEND local_libs CppUnitLite)
list(APPEND full_libs CppUnitLite)
# Build grouped tests
gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label
"tests/test*.cpp" check "Test" # Standard for all tests
"${local_libs}"
"${full_libs}" "${excluded_tests}" # Pass in linking and exclusion lists
${is_test}) # Set all as tests
endfunction()
# Macro for adding categorized timing scripts in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with boost
# Arguments:
# - subdir The name of the category for this timing script
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_srcs A list of timing files that should not be compiled - use for debugging
macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs)
# Subdirectory target for timing - does not actually execute the scripts
add_custom_target(timing.${subdir})
set(is_test FALSE)
# Build grouped benchmarks
gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label
"tests/time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts
"${local_libs}" "${full_libs}" "${excluded_srcs}" # Pass in linking and exclusion lists
${is_test}) # Treat as not a test
endmacro()
# Macro for adding executables matching a pattern - builds one executable for
# each file matching the pattern. These exectuables are automatically linked
# with boost.
# Arguments:
# - pattern The glob pattern to match source files
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_srcs A list of timing files that should not be compiled - use for debugging
function(gtsam_add_executables pattern local_libs full_libs excluded_srcs)
set(is_test FALSE)
if(NOT excluded_srcs)
set(excluded_srcs "")
# Enable make check (http://www.cmake.org/Wiki/CMakeEmulateMakeCheck)
if(GTSAM_BUILD_TESTS)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
endif()
# Build executables
gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test})
endfunction()
# Add examples target
add_custom_target(examples)
# Add timing target
add_custom_target(timing)
# Include obsolete macros - will be removed in the near future
include(GtsamTestingObsolete)
# Implementations of this file's macros:
macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
if(GTSAM_BUILD_TESTS)
# Add group target if it doesn't already exist
if(NOT TARGET check.${groupName})
add_custom_target(check.${groupName} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
endif()
# General-purpose script for adding tests with categories and linking options
macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test)
# Get all script files
set(script_files "")
foreach(one_pattern ${pattern})
file(GLOB one_script_files "${one_pattern}")
list(APPEND script_files "${one_script_files}")
endforeach()
file(GLOB script_files ${globPatterns})
# Remove excluded scripts from the list
set(exclusions "") # Need to copy out exclusion list for logic to work
foreach(one_exclusion ${excluded_srcs})
file(GLOB one_exclusion_srcs "${one_exclusion}")
list(APPEND exclusions "${one_exclusion_srcs}")
endforeach()
if(exclusions)
list(REMOVE_ITEM script_files ${exclusions})
endif(exclusions)
if(NOT "${excludedFiles}" STREQUAL "")
file(GLOB excludedFilePaths ${excludedFiles})
if("${excludedFilePaths}" STREQUAL "")
message(WARNING "The pattern '${excludedFiles}' for excluding tests from group ${groupName} did not match any files")
else()
list(REMOVE_ITEM script_files ${excludedFilePaths})
endif()
endif()
# Separate into source files and headers
# Separate into source files and headers (allows for adding headers to show up in
# MSVC and Xcode projects).
set(script_srcs "")
set(script_headers "")
foreach(script_file ${script_files})
foreach(script_file IN ITEMS ${script_files})
get_filename_component(script_ext ${script_file} EXT)
if(script_ext MATCHES "(h|H)")
list(APPEND script_headers ${script_file})
@ -114,93 +148,123 @@ macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name l
endif()
endforeach()
# Don't put test files in folders in MSVC and Xcode because they're already grouped
source_group("" FILES ${script_srcs} ${script_headers})
# Add targets and dependencies for each script
if(NOT "${group}" STREQUAL "")
message(STATUS "Adding ${pretty_prefix_name}s in ${group}")
endif()
if(NOT GTSAM_SINGLE_TEST_EXE)
# Default for Makefiles - each test in its own executable
foreach(script_src IN ITEMS ${script_srcs})
# Get test base name
get_filename_component(script_name ${script_src} NAME_WE)
# Create exe's for each script, unless we're in SINGLE_TEST_EXE mode
if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE)
foreach(script_src ${script_srcs})
get_filename_component(script_base ${script_src} NAME_WE)
if (script_base) # Check for null filenames and headers
set( script_bin ${script_base} )
message(STATUS "Adding ${pretty_prefix_name} ${script_bin}")
add_executable(${script_bin} ${script_src} ${script_headers})
if(NOT "${target_prefix}" STREQUAL "")
if(NOT "${group}" STREQUAL "")
add_dependencies(${target_prefix}.${group} ${script_bin})
endif()
add_dependencies(${target_prefix} ${script_bin})
# Add executable
add_executable(${script_name} ${script_src} ${script_headers})
target_link_libraries(${script_name} CppUnitLite ${linkLibraries})
# Add target dependencies
add_test(NAME ${script_name} COMMAND ${script_name})
add_dependencies(check.${groupName} ${script_name})
add_dependencies(check ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Disable building during make all/install
if (GTSAM_DISABLE_TESTS_ON_INSTALL)
set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
# Exclude from 'make all' and 'make install'
set_target_properties(${script_name} PROPERTIES EXCLUDE_FROM_ALL ON)
if (is_test)
add_test(NAME ${script_base} COMMAND ${script_bin})
endif()
# Linking and dependendencies
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES})
# Configure target folder (for MSVC and Xcode)
set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}")
endforeach()
else()
target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES})
endif()
# Default on MSVC and XCode - combine test group into a single exectuable
set(target_name check_${groupName}_program)
# Add .run target
if(NOT MSVC)
add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN})
endif()
# Add executable
add_executable(${target_name} ${script_srcs} ${script_headers})
target_link_libraries(${target_name} CppUnitLite ${linkLibraries})
# Set up Visual Studio folders
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
endif()
endforeach(script_src)
if(MSVC)
source_group("" FILES ${script_srcs} ${script_headers})
endif()
else()
# Create single unit test exe from all test scripts
set(script_bin ${target_prefix}_${group}_prog)
add_executable(${script_bin} ${script_srcs} ${script_headers})
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${Boost_LIBRARIES})
else()
target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs})
endif()
# Only have a main function in one script
# Only have a main function in one script - use preprocessor
set(rest_script_srcs ${script_srcs})
list(REMOVE_AT rest_script_srcs 0)
set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main")
set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=inline no_main")
# Add target dependencies
add_test(NAME ${target_name} COMMAND ${target_name})
add_dependencies(check.${groupName} ${target_name})
add_dependencies(check ${target_name})
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Add test
add_dependencies(${target_prefix}.${group} ${script_bin})
add_dependencies(${target_prefix} ${script_bin})
add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin})
# Exclude from 'make all' and 'make install'
set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON)
# Disable building during make all/install
if (GTSAM_DISABLE_TESTS_ON_INSTALL)
set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
# Set up Visual Studio folders
if(MSVC)
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
source_group("" FILES ${script_srcs} ${script_headers})
# Configure target folder (for MSVC and Xcode)
set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests")
endif()
endif()
endmacro()
macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName buildWithAll)
# Get all script files
file(GLOB script_files ${globPatterns})
# Remove excluded scripts from the list
if(NOT "${excludedFiles}" STREQUAL "")
file(GLOB excludedFilePaths ${excludedFiles})
if("${excludedFilePaths}" STREQUAL "")
message(WARNING "The script exclusion pattern '${excludedFiles}' did not match any files")
else()
list(REMOVE_ITEM script_files ${excludedFilePaths})
endif()
endif()
# Separate into source files and headers (allows for adding headers to show up in
# MSVC and Xcode projects).
set(script_srcs "")
set(script_headers "")
foreach(script_file IN ITEMS ${script_files})
get_filename_component(script_ext ${script_file} EXT)
if(script_ext MATCHES "(h|H)")
list(APPEND script_headers ${script_file})
else()
list(APPEND script_srcs ${script_file})
endif()
endforeach()
# Don't put test files in folders in MSVC and Xcode because they're already grouped
source_group("" FILES ${script_srcs} ${script_headers})
# Create executables
foreach(script_src IN ITEMS ${script_srcs})
# Get script base name
get_filename_component(script_name ${script_src} NAME_WE)
# Add executable
add_executable(${script_name} ${script_src} ${script_headers})
target_link_libraries(${script_name} ${linkLibraries})
# Add target dependencies
add_dependencies(${groupName} ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Exclude from all or not - note weird variable assignment because we're in a macro
set(buildWithAll_on ${buildWithAll})
if(NOT buildWithAll_on)
# Exclude from 'make all' and 'make install'
set_target_properties("${script_name}" PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
# Configure target folder (for MSVC and Xcode)
set_property(TARGET ${script_name} PROPERTY FOLDER "${groupName}")
endforeach()
endmacro()

195
cmake/GtsamTestingObsolete.cmake Normal file
View File

@ -0,0 +1,195 @@
# Macro for adding categorized tests in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with CppUnitLite and boost
# Arguments:
# - subdir The name of the category for this test
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_tests A list of test files that should not be compiled - use for debugging
function(gtsam_add_subdir_tests subdir local_libs full_libs excluded_tests)
# Subdirectory target for tests
add_custom_target(check.${subdir} COMMAND ${CMAKE_CTEST_COMMAND} -C $<CONFIGURATION> --output-on-failure)
set(is_test TRUE)
# Put check target in Visual Studio solution folder
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET check.${subdir} PROPERTY FOLDER "${relative_path}")
# Link with CppUnitLite - pulled from gtsam installation
list(APPEND local_libs CppUnitLite)
list(APPEND full_libs CppUnitLite)
# Build grouped tests
gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label
"tests/test*.cpp" check "Test" # Standard for all tests
"${local_libs}"
"${full_libs}" "${excluded_tests}" # Pass in linking and exclusion lists
${is_test}) # Set all as tests
endfunction()
# Macro for adding categorized timing scripts in a "tests" folder, with
# optional exclusion of tests and convenience library linking options
#
# By default, all tests are linked with boost
# Arguments:
# - subdir The name of the category for this timing script
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_srcs A list of timing files that should not be compiled - use for debugging
macro(gtsam_add_subdir_timing subdir local_libs full_libs excluded_srcs)
# Subdirectory target for timing - does not actually execute the scripts
add_custom_target(timing.${subdir})
set(is_test FALSE)
# Build grouped benchmarks
gtsam_add_grouped_scripts("${subdir}" # Use subdirectory as group label
"tests/time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts
"${local_libs}" "${full_libs}" "${excluded_srcs}" # Pass in linking and exclusion lists
${is_test}) # Treat as not a test
endmacro()
# Macro for adding executables matching a pattern - builds one executable for
# each file matching the pattern. These exectuables are automatically linked
# with boost.
# Arguments:
# - pattern The glob pattern to match source files
# - local_libs A list of convenience libraries to use (if GTSAM_BUILD_CONVENIENCE_LIBRARIES is true)
# - full_libs The main library to link against if not using convenience libraries
# - excluded_srcs A list of timing files that should not be compiled - use for debugging
function(gtsam_add_executables pattern local_libs full_libs excluded_srcs)
set(is_test FALSE)
if(NOT excluded_srcs)
set(excluded_srcs "")
endif()
# Build executables
gtsam_add_grouped_scripts("" "${pattern}" "" "Executable" "${local_libs}" "${full_libs}" "${excluded_srcs}" ${is_test})
endfunction()
# General-purpose script for adding tests with categories and linking options
macro(gtsam_add_grouped_scripts group pattern target_prefix pretty_prefix_name local_libs full_libs excluded_srcs is_test)
# Print warning about using this obsolete function
message(AUTHOR_WARNING "Warning: Please see GtsamTesting.cmake - obsolete cmake cmake macro for creating unit tests, examples, and scripts was called. This will be removed in the future. The new macros are much easier anyway!!")
# Get all script files
set(script_files "")
foreach(one_pattern ${pattern})
file(GLOB one_script_files "${one_pattern}")
list(APPEND script_files "${one_script_files}")
endforeach()
# Remove excluded scripts from the list
set(exclusions "") # Need to copy out exclusion list for logic to work
foreach(one_exclusion ${excluded_srcs})
file(GLOB one_exclusion_srcs "${one_exclusion}")
list(APPEND exclusions "${one_exclusion_srcs}")
endforeach()
if(exclusions)
list(REMOVE_ITEM script_files ${exclusions})
endif(exclusions)
# Separate into source files and headers
set(script_srcs "")
set(script_headers "")
foreach(script_file ${script_files})
get_filename_component(script_ext ${script_file} EXT)
if(script_ext MATCHES "(h|H)")
list(APPEND script_headers ${script_file})
else()
list(APPEND script_srcs ${script_file})
endif()
endforeach()
# Add targets and dependencies for each script
if(NOT "${group}" STREQUAL "")
message(STATUS "Adding ${pretty_prefix_name}s in ${group}")
endif()
# Create exe's for each script, unless we're in SINGLE_TEST_EXE mode
if(NOT is_test OR NOT GTSAM_SINGLE_TEST_EXE)
foreach(script_src ${script_srcs})
get_filename_component(script_base ${script_src} NAME_WE)
if (script_base) # Check for null filenames and headers
set( script_bin ${script_base} )
message(STATUS "Adding ${pretty_prefix_name} ${script_bin}")
add_executable(${script_bin} ${script_src} ${script_headers})
if(NOT "${target_prefix}" STREQUAL "")
if(NOT "${group}" STREQUAL "")
add_dependencies(${target_prefix}.${group} ${script_bin})
endif()
add_dependencies(${target_prefix} ${script_bin})
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Disable building during make all/install
if (GTSAM_DISABLE_TESTS_ON_INSTALL)
set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
if (is_test)
add_test(NAME ${script_base} COMMAND ${script_bin})
endif()
# Linking and dependendencies
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${GTSAM_BOOST_LIBRARIES})
else()
target_link_libraries(${script_bin} ${full_libs} ${GTSAM_BOOST_LIBRARIES})
endif()
# Add .run target
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_bin}.run ${EXECUTABLE_OUTPUT_PATH}${script_bin} ${ARGN})
endif()
# Set up Visual Studio folders
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
endif()
endforeach(script_src)
if(MSVC)
source_group("" FILES ${script_srcs} ${script_headers})
endif()
else()
# Create single unit test exe from all test scripts
set(script_bin ${target_prefix}_${group}_prog)
add_executable(${script_bin} ${script_srcs} ${script_headers})
if (GTSAM_BUILD_CONVENIENCE_LIBRARIES)
target_link_libraries(${script_bin} ${local_libs} ${Boost_LIBRARIES})
else()
target_link_libraries(${script_bin} ${Boost_LIBRARIES} ${full_libs})
endif()
# Only have a main function in one script
set(rest_script_srcs ${script_srcs})
list(REMOVE_AT rest_script_srcs 0)
set_property(SOURCE ${rest_script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "main=static no_main")
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
# Add test
add_dependencies(${target_prefix}.${group} ${script_bin})
add_dependencies(${target_prefix} ${script_bin})
add_test(NAME ${target_prefix}.${group} COMMAND ${script_bin})
# Disable building during make all/install
if (GTSAM_DISABLE_TESTS_ON_INSTALL)
set_target_properties(${script_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
# Set up Visual Studio folders
if(MSVC)
file(RELATIVE_PATH relative_path "${PROJECT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}")
set_property(TARGET ${script_bin} PROPERTY FOLDER "${relative_path}")
source_group("" FILES ${script_srcs} ${script_headers})
endif()
endif()
endmacro()

92
cmake/README.html Normal file
View File

@ -0,0 +1,92 @@
<h1>GTSAMCMakeTools</h1>
<p>This is the collection of GTSAM CMake tools that may be useful in external projects. The way to use this collection is by first making a find_package call:</p>
<pre><code>find_package(GTSAMCMakeTools)
</code></pre>
<p>which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable. After that, you may include the files you would like to use. These files and the functions they define are explained below.</p>
<h2>GtsamBuildTypes</h2>
<pre><code>include(GtsamBuildTypes)
</code></pre>
<p>Including this file immediately sets up the following build types and a drop-down list in cmake-gui:</p>
<ul>
<li><code>Debug</code></li>
<li><code>Release</code></li>
<li><code>RelWithDebInfo</code></li>
<li><code>Profiling</code>: All optimizations enabled and minimal debug symbols</li>
<li><code>Timing</code>: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation</li>
</ul>
<p>It also configures several minor details, as follows:</p>
<ul>
<li>The compile flag <code>-ftemplate-depth=1024</code> is set for newer versions of Clang to handle complex templates.</li>
<li>On Windows, executable and dll output paths are set to <code>${CMAKE_BINARY_DIR}/bin</code> and import library output to <code>${CMAKE_BINARY_DIR}/bin</code>.</li>
</ul>
<p>It defines the following functions:</p>
<ul>
<li><code>gtsam_assign_source_folders( [files] )</code> Organizes files in the IDE into folders to reflect the actual directory structure of those files. Folders will be determined relative to the current source folder when this function is called.</li>
<li><code>gtsam_assign_all_source_folders()</code> Calls <code>gtsam_assign_source_folders</code> on all cpp, c, and h files recursively in the current source folder.</li>
</ul>
<h2>GtsamTesting</h2>
<pre><code>include(GtsamTesting)
</code></pre>
<p>Defines two useful functions for creating CTest unit tests. Also immediately creates a <code>check</code> target that builds and runs all unit tests.</p>
<ul>
<li>
<p><code>gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries)</code> Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the tests to create. Tests are assigned into a group name so they can easily by run independently with a make target. Running 'make check' builds and runs all tests.</p>
<p>Usage example:</p>
<pre><code>gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib")
</code></pre>
<p>Arguments:</p>
<pre><code>groupName: A name that will allow this group of tests to be run independently, e.g.
'basic' causes a 'check.basic' target to be created to run this test
group.
globPatterns: The list of files or glob patterns from which to create unit tests, with
one test created for each cpp file. e.g. "test*.cpp", or
"testA*.cpp;testB*.cpp;testOneThing.cpp".
excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp".
Pass an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to in addition to CppUnitLite.
</code></pre>
</li>
<li>
<p><code>gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)</code> Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.</p>
<p>Usage example:</p>
<pre><code>gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
</code></pre>
<p>Arguments:</p>
<pre><code>globPatterns: The list of files or glob patterns from which to create unit tests, with
one test created for each cpp file. e.g. "*.cpp", or
"A*.cpp;B*.cpp;MyExample.cpp".
excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to.
</code></pre>
</li>
</ul>
<h2>GtsamMatlabWrap</h2>
<pre><code>include(GtsamMatlabWrap)
</code></pre>
<p>Defines functions for generating MATLAB wrappers. Also immediately creates several CMake options for configuring the wrapper.</p>
<ul>
<li>
<p><code>wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)</code> Generates wrap code and compiles the wrapper.</p>
<p>Usage example:</p>
<pre><code>`wrap_and_install_library("lba.h" "" "" "")`
</code></pre>
<p>Arguments:</p>
<pre><code>interfaceHeader: The relative or absolute path to the wrapper interface
definition file.
linkLibraries: Any *additional* libraries to link. Your project library
(e.g. `lba`), libraries it depends on, and any necessary
MATLAB libraries will be linked automatically. So normally,
leave this empty.
extraIncludeDirs: Any *additional* include paths required by dependent
libraries that have not already been added by
include_directories. Again, normally, leave this empty.
extraMexFlags: Any *additional* flags to pass to the compiler when building
the wrap code. Normally, leave this empty.
</code></pre>
</li>
</ul>
<h2>GtsamMakeConfigFile</h2>
<pre><code>include(GtsamMakeConfigFile)
</code></pre>
<p>Defines a function for generating a config file so your project may be found with the CMake <code>find_package</code> function. TODO: Write documentation.</p>

105
cmake/README.md Normal file
View File

@ -0,0 +1,105 @@
GTSAMCMakeTools
===============
This is the collection of GTSAM CMake tools that may be useful in external projects. The way to use this collection is by first making a find_package call:
find_package(GTSAMCMakeTools)
which will add a directory containing the GTSAM CMake tools to the CMAKE_MODULE_PATH variable. After that, you may include the files you would like to use. These files and the functions they define are explained below.
GtsamBuildTypes
---------------
include(GtsamBuildTypes)
Including this file immediately sets up the following build types and a drop-down list in cmake-gui:
* `Debug`
* `Release`
* `RelWithDebInfo`
* `Profiling`: All optimizations enabled and minimal debug symbols
* `Timing`: Defines the symbol GTSAM_ENABLE_TIMING for using GTSAM timing instrumentation
It also configures several minor details, as follows:
* The compile flag `-ftemplate-depth=1024` is set for newer versions of Clang to handle complex templates.
* On Windows, executable and dll output paths are set to `${CMAKE_BINARY_DIR}/bin` and import library output to `${CMAKE_BINARY_DIR}/lib`.
It defines the following functions:
* `gtsam_assign_source_folders( [files] )` Organizes files in the IDE into folders to reflect the actual directory structure of those files. Folders will be determined relative to the current source folder when this function is called.
* `gtsam_assign_all_source_folders()` Calls `gtsam_assign_source_folders` on all cpp, c, and h files recursively in the current source folder.
GtsamTesting
------------
include(GtsamTesting)
Defines two useful functions for creating CTest unit tests. Also immediately creates a `check` target that builds and runs all unit tests.
* `gtsamAddTestsGlob(groupName globPatterns excludedFiles linkLibraries)` Add a group of unit tests. A list of unit test .cpp files or glob patterns specifies the tests to create. Tests are assigned into a group name so they can easily by run independently with a make target. Running 'make check' builds and runs all tests.
Usage example:
gtsamAddTestsGlob(basic "test*.cpp" "testBroken.cpp" "gtsam;GeographicLib")
Arguments:
groupName: A name that will allow this group of tests to be run independently, e.g.
'basic' causes a 'check.basic' target to be created to run this test
group.
globPatterns: The list of files or glob patterns from which to create unit tests, with
one test created for each cpp file. e.g. "test*.cpp", or
"testA*.cpp;testB*.cpp;testOneThing.cpp".
excludedFiles: A list of files or globs to exclude, e.g. "testC*.cpp;testBroken.cpp".
Pass an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to in addition to CppUnitLite.
* `gtsamAddExamplesGlob(globPatterns excludedFiles linkLibraries)` Add scripts that will serve as examples of how to use the library. A list of files or glob patterns is specified, and one executable will be created for each matching .cpp file. These executables will not be installed. They are build with 'make all' if GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'.
Usage example:
gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib")
Arguments:
globPatterns: The list of files or glob patterns from which to create unit tests, with
one test created for each cpp file. e.g. "*.cpp", or
"A*.cpp;B*.cpp;MyExample.cpp".
excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass
an empty string "" if nothing needs to be excluded.
linkLibraries: The list of libraries to link to.
GtsamMatlabWrap
---------------
include(GtsamMatlabWrap)
Defines functions for generating MATLAB wrappers. Also immediately creates several CMake options for configuring the wrapper.
* `wrap_and_install_library(interfaceHeader linkLibraries extraIncludeDirs extraMexFlags)` Generates wrap code and compiles the wrapper.
Usage example:
`wrap_and_install_library("lba.h" "" "" "")`
Arguments:
interfaceHeader: The relative or absolute path to the wrapper interface
definition file.
linkLibraries: Any *additional* libraries to link. Your project library
(e.g. `lba`), libraries it depends on, and any necessary
MATLAB libraries will be linked automatically. So normally,
leave this empty.
extraIncludeDirs: Any *additional* include paths required by dependent
libraries that have not already been added by
include_directories. Again, normally, leave this empty.
extraMexFlags: Any *additional* flags to pass to the compiler when building
the wrap code. Normally, leave this empty.
GtsamMakeConfigFile
-------------------
include(GtsamMakeConfigFile)
Defines a function for generating a config file so your project may be found with the CMake `find_package` function. TODO: Write documentation.

170
cmake/example_project/CMakeLists.txt
View File

@ -1,173 +1,43 @@
# This file should be used as a template for creating new projects using the CMake tools
# This project has the following features
# - GTSAM linking
# - Boost linking
# - Unit tests via CppUnitLite
# - Automatic detection of sources and headers in subfolders
# - Installation of library and headers
# - Matlab wrap interface with within-project building
# - Use of GTSAM cmake macros
# - Scripts
# - Automatic MATLAB wrapper generation
###################################################################################
# To create your own project, replace "myproject" with the actual name of your project
# To create your own project, replace "example" with the actual name of your project
cmake_minimum_required(VERSION 2.6)
enable_testing()
project(myproject CXX C)
project(example CXX C)
# Add the cmake subfolder to the cmake module path - necessary to use macros
set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${PROJECT_SOURCE_DIR}/cmake")
# Include GTSAM CMake tools
find_package(GTSAMCMakeTools)
include(GtsamBuildTypes) # Load build type flags and default to Debug mode
include(GtsamTesting) # Easy functions for creating unit tests and scripts
include(GtsamMatlabWrap) # Automatic MATLAB wrapper generation
# Ensure that local folder is searched before library folders
include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
###################################################################################
# Create a list of library dependencies
# These will be linked with executables
set(library_deps "")
set(linking_mode "static")
# Find GTSAM components
find_package(GTSAM REQUIRED) # Uses installed package
list(APPEND library_deps gtsam-${linking_mode} gtsam_unstable-${linking_mode})
# Include ransac
find_package(ransac REQUIRED) # Uses installed package
list(APPEND library_deps ransac-${linking_mode})
# Boost - same requirement as gtsam
find_package(Boost 1.43 COMPONENTS
serialization
system
filesystem
thread
date_time
REQUIRED)
list(APPEND library_deps
${Boost_SERIALIZATION_LIBRARY}
${Boost_SYSTEM_LIBRARY}
${Boost_FILESYSTEM_LIBRARY}
${Boost_THREAD_LIBRARY}
${Boost_DATE_TIME_LIBRARY})
include_directories(${Boost_INCLUDE_DIR} ${GTSAM_INCLUDE_DIR} ${ransac_INCLUDE_DIR})
include_directories(${GTSAM_INCLUDE_DIR})
###################################################################################
# List subdirs to process tests/sources
# Each of these will be scanned for new files
set (myproject_subdirs
"." # ensure root folder gets included
stuff
things
)
# loop through subdirs to install and build up source lists
set(myproject_lib_source "")
set(myproject_tests_source "")
set(myproject_scripts_source "")
foreach(subdir ${myproject_subdirs})
# Installing headers
message(STATUS "Installing ${subdir}")
file(GLOB sub_myproject_headers "myproject/${subdir}/*.h")
install(FILES ${sub_myproject_headers} DESTINATION include/myproject/${subdir})
# add sources to main sources list
file(GLOB subdir_srcs "myproject/${subdir}/*.cpp")
list(APPEND myproject_lib_source ${subdir_srcs})
# add tests to main tests list
file(GLOB subdir_test_srcs "myproject/${subdir}/tests/*.cpp")
list(APPEND myproject_tests_source ${subdir_test_srcs})
# add scripts to main tests list
file(GLOB subdir_scripts_srcs "myproject/${subdir}/scripts/*.cpp")
list(APPEND myproject_scripts_source ${subdir_scripts_srcs})
endforeach(subdir)
set(myproject_version ${myproject_VERSION_MAJOR}.${myproject_VERSION_MINOR}.${myproject_VERSION_PATCH})
set(myproject_soversion ${myproject_VERSION_MAJOR})
message(STATUS "GTSAM Version: ${gtsam_version}")
message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
# Build library (static and shared versions)
# Include installed versions
SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
add_library(${PROJECT_NAME}-shared SHARED ${myproject_lib_source})
set_target_properties(${PROJECT_NAME}-shared PROPERTIES
OUTPUT_NAME ${PROJECT_NAME}
CLEAN_DIRECT_OUTPUT 1)
install(TARGETS myproject-shared EXPORT myproject-exports LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin)
list(APPEND myproject_EXPORTED_TARGETS myproject-shared)
add_library(${PROJECT_NAME}-static STATIC ${myproject_lib_source})
set_target_properties(${PROJECT_NAME}-static PROPERTIES
OUTPUT_NAME ${PROJECT_NAME}
CLEAN_DIRECT_OUTPUT 1)
install(TARGETS myproject-static EXPORT myproject-exports ARCHIVE DESTINATION lib)
list(APPEND myproject_EXPORTED_TARGETS myproject-static)
install(TARGETS ${PROJECT_NAME}-shared LIBRARY DESTINATION lib )
install(TARGETS ${PROJECT_NAME}-static ARCHIVE DESTINATION lib )
# Disabled tests - subtract these from the test files
# Note the need for a full path
set(disabled_tests
"dummy"
#"${PROJECT_SOURCE_DIR}/myproject/geometry/tests/testCovarianceEllipse.cpp"
)
list(REMOVE_ITEM myproject_tests_source ${disabled_tests})
# Build static library from common sources
set(CONVENIENCE_LIB_NAME ${PROJECT_NAME})
add_library(${CONVENIENCE_LIB_NAME} STATIC example/PrintExamples.h example/PrintExamples.cpp)
target_link_libraries(${CONVENIENCE_LIB_NAME} gtsam)
###################################################################################
# Build tests
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND})
foreach(test_src_file ${myproject_tests_source})
get_filename_component(test_base ${test_src_file} NAME_WE)
message(STATUS "Adding test ${test_src_file} with base name ${test_base}" )
add_executable(${test_base} ${test_src_file})
target_link_libraries(${test_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite)
add_test(${test_base} ${EXECUTABLE_OUTPUT_PATH}/${test_base})
add_custom_target(${test_base}.run ${test_base} ${ARGN})
add_dependencies(check ${test_base})
endforeach(test_src_file)
# Build scripts
foreach(script_src_file ${myproject_scripts_source})
get_filename_component(script_base ${script_src_file} NAME_WE)
message(STATUS "Adding script ${script_src_file} with base name ${script_base}" )
add_executable(${script_base} ${script_src_file})
target_link_libraries(${script_base} ${PROJECT_NAME}-${linking_mode} ${library_deps} CppUnitLite)
add_custom_target(${script_base}.run ${script_base} ${ARGN})
endforeach(script_src_file)
# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsamAddTestsGlob("example" "tests/test*.cpp" "" "${CONVENIENCE_LIB_NAME}")
###################################################################################
# Matlab wrapping
include(GtsamMatlabWrap)
set(MEX_COMMAND "mex" CACHE STRING "Command to use for executing mex (if on path, 'mex' will work)")
set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
set(MYPROJECT_TOOLBOX_DIR "../matlab/myproject" CACHE PATH "Install folder for matlab toolbox - defaults to inside project")
set(WRAP_HEADER_PATH "${GTSAM_DIR}/../../../include")
set(MYPROJECT_TOOLBOX_FLAGS
${GTSAM_BUILD_MEX_BINARY_FLAGS} -I${PROJECT_SOURCE_DIR} -I${PROJECT_SOURCE_DIR}/myproject -I${Boost_INCLUDE_DIR} -I${MEX_INCLUDE_ROOT} -I${GTSAM_INCLUDE_DIR} -I${WRAP_HEADER_PATH} -Wl,-rpath,${CMAKE_BINARY_DIR}:${CMAKE_INSTALL_PREFIX}/lib)
set(MYPROJECT_LIBRARY_DEPS gtsam gtsam_unstable ransac myproject)
set(GTSAM_BUILD_MEX_BIN ON)
# Function to setup codegen, building and installation of the wrap toolbox
# This wrap setup function assumes that the toolbox will be installed directly,
# with predictable matlab.h sourcing
# params:
# moduleName : the name of the module, interface file must be called moduleName.h
# mexFlags : Compilation flags to be passed to the mex compiler
# modulePath : relative path to module markup header file (called moduleName.h)
# otherLibraries : list of library targets this should depend on
# toolboxPath : the directory in which to generate/build wrappers
# wrap_header_path : path to the installed wrap header
wrap_library_generic(myproject "${MYPROJECT_TOOLBOX_FLAGS}" "" "${MYPROJECT_LIBRARY_DEPS}" "${MYPROJECT_TOOLBOX_DIR}" "${WRAP_HEADER_PATH}")
# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsamAddExamplesGlob("*.cpp" "" "${CONVENIENCE_LIB_NAME}")
###################################################################################
# Create Install config and export files
# This config file takes the place of FindXXX.cmake scripts
include(GtsamMakeConfigFile)
GtsamMakeConfigFile(myproject)
export(TARGETS ${myproject_EXPORTED_TARGETS} FILE myproject-exports.cmake)
# Build MATLAB wrapper (CMake tracks the dependecy to link with GTSAM through our project's static library)
wrap_and_install_library("example.h" "${CONVENIENCE_LIB_NAME}" "" "")

23
cmake/example_project/SayGoodbye.cpp Normal file
View File

@ -0,0 +1,23 @@
/* ----------------------------------------------------------------------------
* 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 SayGoodbye.cpp
* @brief Example script for example project
* @author Richard Roberts
*/
#include <example/PrintExamples.h>
int main(int argc, char *argv[]) {
example::PrintExamples().sayGoodbye();
return 0;
}

23
cmake/example_project/SayHello.cpp Normal file
View File

@ -0,0 +1,23 @@
/* ----------------------------------------------------------------------------
* 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 SayHello.cpp
* @brief Example script for example project
* @author Richard Roberts
*/
#include <example/PrintExamples.h>
int main(int argc, char *argv[]) {
example::PrintExamples().sayHello();
return 0;
}

28
cmake/example_project/example.h Normal file
View File

@ -0,0 +1,28 @@
/* ----------------------------------------------------------------------------
* 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 example.h
* @brief Example wrapper interface file
* @author Richard Roberts
*/
// This is an interface file for automatic MATLAB wrapper generation. See
// gtsam.h for full documentation and more examples.
namespace example {
class PrintExamples {
void sayHello() const;
void sayGoodbye() const;
};
}

44
cmake/example_project/example/PrintExamples.cpp Normal file
View File

@ -0,0 +1,44 @@
/* ----------------------------------------------------------------------------
* 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 print_examples.cpp
* @brief Example library file
* @author Richard Roberts
*/
#include <iostream>
#include <example/PrintExamples.h>
namespace example {
void PrintExamples::sayHello() const {
std::cout << internal::getHelloString() << std::endl;
}
void PrintExamples::sayGoodbye() const {
std::cout << internal::getGoodbyeString() << std::endl;
}
namespace internal {
std::string getHelloString() {
return "Hello!";
}
std::string getGoodbyeString() {
return "See you soon!";
}
} // namespace internal
} // namespace example

41
cmake/example_project/example/PrintExamples.h Normal file
View File

@ -0,0 +1,41 @@
/* ----------------------------------------------------------------------------
* 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 print_examples.h
* @brief Example library file
* @author Richard Roberts
*/
#pragma once
#include <string>
namespace example {
class PrintExamples {
public:
/// Print a greeting
void sayHello() const;
/// Print a farewell
void sayGoodbye() const;
};
namespace internal {
std::string getHelloString();
std::string getGoodbyeString();
} // namespace internal
} // namespace example

43
cmake/example_project/tests/testExample.cpp Normal file
View File

@ -0,0 +1,43 @@
/* ----------------------------------------------------------------------------
* 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 testExample.cpp
* @brief Unit tests for example
* @author Richard Roberts
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <example/PrintExamples.h>
using namespace gtsam;
TEST(Example, HelloString) {
const std::string expectedString = "Hello!";
EXPECT(assert_equal(expectedString, example::internal::getHelloString()));
}
TEST(Example, GoodbyeString) {
const std::string expectedString = "See you soon!";
EXPECT(assert_equal(expectedString, example::internal::getGoodbyeString()));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

38
cmake/example_project_simple/CMakeLists.txt
View File

@ -1,38 +0,0 @@
# This file should be used as a template for creating new projects using the CMake tools
# This project has the following features
# - GTSAM linking
# - Unit tests via CppUnitLite
# - Scripts
###################################################################################
# To create your own project, replace "myproject" with the actual name of your project
cmake_minimum_required(VERSION 2.6)
enable_testing()
project(myproject CXX C)
# Add the cmake subfolder to the cmake module path - necessary to use macros
list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
# Ensure that local folder is searched before library folders
include_directories(BEFORE "${PROJECT_SOURCE_DIR}")
# Load build type flags and default to Debug mode
include(GtsamBuildTypes)
###################################################################################
# Find GTSAM components
find_package(GTSAM REQUIRED) # Uses installed package
include_directories(${GTSAM_INCLUDE_DIR})
###################################################################################
# Build static library from common sources
add_library(${PROJECT_NAME} STATIC ${PROJECT_NAME}/MySourceFiles.cpp)
target_link_libraries(${PROJECT_NAME} gtsam-shared)
###################################################################################
# Build tests (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsam_add_subdir_tests(${PROJECT_NAME} "${PROJECT_NAME}" "${PROJECT_NAME}" "")
###################################################################################
# Build scripts (CMake tracks the dependecy to link with GTSAM through our project's static library)
gtsam_add_executables("${PROJECT_NAME}/myScripts.cpp" "${PROJECT_NAME}" "${PROJECT_NAME}" "")

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 *)

2527
doc/gtsam-coordinate-frames.lyx Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
doc/gtsam-coordinate-frames.pdf Normal file
View File

Binary file not shown.

46
examples/CMakeLists.txt
View File

@ -1,42 +1,8 @@
if(NOT MSVC)
add_custom_target(examples)
endif()
# Build example executables
FILE(GLOB example_srcs "*.cpp")
set (excluded_examples #"")
"${CMAKE_CURRENT_SOURCE_DIR}/DiscreteBayesNet_FG.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/UGM_chain.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/UGM_small.cpp"
"${CMAKE_CURRENT_SOURCE_DIR}/elaboratePoint2KalmanFilter.cpp"
set (excluded_examples
DiscreteBayesNet_FG.cpp
UGM_chain.cpp
UGM_small.cpp
elaboratePoint2KalmanFilter.cpp
)
list(REMOVE_ITEM example_srcs ${excluded_examples})
foreach(example_src ${example_srcs} )
get_filename_component(example_base ${example_src} NAME_WE)
set( example_bin ${example_base} )
message(STATUS "Adding Example ${example_bin}")
if(NOT MSVC)
add_dependencies(examples ${example_bin})
endif()
add_executable(${example_bin} ${example_src})
# Disable building during make all/install
if (GTSAM_DISABLE_EXAMPLES_ON_INSTALL)
set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
target_link_libraries(${example_bin} ${gtsam-default} ${Boost_PROGRAM_OPTIONS_LIBRARY})
if(NOT MSVC)
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
endif()
# Set up Visual Studio folder
if(MSVC)
set_property(TARGET ${example_bin} PROPERTY FOLDER "Examples")
endif()
endforeach(example_src)
gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}")

4
examples/CameraResectioning.cpp
View File

@ -48,7 +48,7 @@ public:
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(pose, *K_);
Point2 reprojectionError(camera.project(P_, H) - p_);
Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
return reprojectionError.vector();
}
};
@ -70,7 +70,7 @@ int main(int argc, char* argv[]) {
/* 2. add factors to the graph */
// add measurement factors
SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor;
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,

1
examples/Data/VO_calibration00.txt Normal file
View File

@ -0,0 +1 @@
718.856 718.856 0.0 607.1928 185.2157 0.5371657189

1
examples/Data/VO_calibration00s.txt Normal file
View File

@ -0,0 +1 @@
718.856 718.856 0.0 607.1928 185.2157 0.5371657189

135
examples/Data/VO_camera_poses00.txt Normal file
View File

@ -0,0 +1,135 @@
0 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
1 0.99999 -0.00268679 -0.00354618 6.43221e-05 0.00267957 0.999994 -0.00204036 -0.0073023 0.00355164 0.00203084 0.999992 0.676456 0 0 0 1
2 0.999969 -0.00120771 -0.00772489 -0.0100328 0.00117985 0.999993 -0.003611 -0.0111185 0.00772919 0.00360178 0.999964 1.37125 0 0 0 1
3 0.999931 -0.00128098 -0.0117006 -0.0237327 0.00122052 0.999986 -0.00517227 -0.0136538 0.0117071 0.00515763 0.999918 2.08563 0 0 0 1
4 0.99986 5.79321e-05 -0.0167106 -0.0402272 -0.000155312 0.999983 -0.00582618 -0.0194327 0.01671 0.00582796 0.999843 2.81528 0 0 0 1
5 0.999772 -0.00118366 -0.0213077 -0.0572378 0.0010545 0.999981 -0.00607208 -0.0278191 0.0213145 0.00604822 0.999755 3.56204 0 0 0 1
6 0.999662 0.000544425 -0.0259946 -0.081545 -0.000735472 0.999973 -0.00734051 -0.0358844 0.0259899 0.00735714 0.999635 4.32265 0 0 0 1
7 0.999513 0.0032602 -0.0310324 -0.112137 -0.0035101 0.999962 -0.00800188 -0.0447209 0.0310051 0.00810691 0.999486 5.09668 0 0 0 1
8 0.999361 0.00349173 -0.0355658 -0.143594 -0.00372162 0.999973 -0.00639979 -0.0532611 0.0355425 0.00652807 0.999347 5.88701 0 0 0 1
9 0.999185 0.00268131 -0.040271 -0.176401 -0.0028332 0.999989 -0.00371493 -0.0632884 0.0402606 0.003826 0.999182 6.6897 0 0 0 1
10 0.99903 0.00226305 -0.0439747 -0.211687 -0.00231163 0.999997 -0.00105382 -0.072362 0.0439722 0.00115445 0.999032 7.50361 0 0 0 1
11 0.998896 0.00366482 -0.0468376 -0.254125 -0.00374515 0.999992 -0.00162734 -0.0820263 0.0468312 0.00180096 0.998901 8.32333 0 0 0 1
12 0.998775 0.00304285 -0.0493866 -0.295424 -0.00313866 0.999993 -0.00186268 -0.0885739 0.0493806 0.00201541 0.998778 9.15211 0 0 0 1
13 0.998682 7.09894e-05 -0.0513155 -0.334647 -0.000203775 0.999997 -0.00258241 -0.0938889 0.0513152 0.00258946 0.998679 9.98839 0 0 0 1
14 0.998565 -8.82523e-05 -0.0535542 -0.380835 -9.36659e-06 0.999998 -0.00182255 -0.10173 0.0535542 0.00182044 0.998563 10.832 0 0 0 1
15 0.998481 -0.00146793 -0.0550718 -0.429135 0.0013525 0.999997 -0.00213307 -0.111427 0.0550748 0.00205535 0.99848 11.687 0 0 0 1
16 0.998373 0.000738731 -0.0570218 -0.483426 -0.000993083 0.99999 -0.00443241 -0.122139 0.0570179 0.00448183 0.998363 12.5483 0 0 0 1
17 0.998285 0.00120595 -0.0585258 -0.540056 -0.00162301 0.999974 -0.00707907 -0.132598 0.0585158 0.00716191 0.998261 13.4179 0 0 0 1
18 0.998165 0.00516151 -0.060337 -0.6023 -0.00570195 0.999945 -0.00878826 -0.143753 0.0602883 0.00911617 0.998139 14.2952 0 0 0 1
19 0.998101 0.00610094 -0.0612993 -0.66308 -0.00663017 0.999942 -0.00843386 -0.157854 0.0612443 0.00882427 0.998084 15.1802 0 0 0 1
20 0.998014 0.0052997 -0.0627662 -0.722045 -0.00574767 0.999959 -0.0069587 -0.172847 0.0627268 0.00730564 0.998004 16.074 0 0 0 1
21 0.99792 0.00591748 -0.0641975 -0.78346 -0.00627924 0.999966 -0.00543487 -0.186221 0.0641631 0.00582667 0.997922 16.9738 0 0 0 1
22 0.997857 0.00547694 -0.0651993 -0.845347 -0.00584101 0.999968 -0.00539455 -0.199741 0.0651677 0.00576382 0.997858 17.8786 0 0 0 1
23 0.997737 0.00536917 -0.0670282 -0.908218 -0.00579979 0.999964 -0.0062316 -0.212775 0.0669924 0.00660624 0.997732 18.7877 0 0 0 1
24 0.997663 0.00386695 -0.0682185 -0.971291 -0.00435203 0.999966 -0.00696344 -0.226442 0.0681893 0.00724406 0.997646 19.7046 0 0 0 1
25 0.997629 0.00410637 -0.0687004 -1.03663 -0.00448288 0.999976 -0.00532714 -0.239555 0.0686769 0.00562249 0.997623 20.6257 0 0 0 1
26 0.997617 0.00588773 -0.0687501 -1.10557 -0.0060349 0.99998 -0.00193325 -0.254273 0.0687373 0.00234355 0.997632 21.55 0 0 0 1
27 0.997662 0.00693766 -0.0679906 -1.17297 -0.00682806 0.999975 0.0018442 -0.26563 0.0680017 -0.00137565 0.997684 22.4875 0 0 0 1
28 0.997774 0.00579785 -0.0664343 -1.23728 -0.00550265 0.999974 0.00462554 -0.271962 0.0664594 -0.00424968 0.99778 23.4285 0 0 0 1
29 0.997872 0.00589563 -0.0649408 -1.30214 -0.00556012 0.99997 0.00534586 -0.277922 0.0649704 -0.0049734 0.997875 24.3732 0 0 0 1
30 0.997958 0.00627024 -0.0635595 -1.36462 -0.00612984 0.999978 0.00240374 -0.285335 0.0635732 -0.00200922 0.997975 25.314 0 0 0 1
31 0.998004 0.00714074 -0.0627411 -1.42783 -0.00731158 0.99997 -0.00249375 -0.293171 0.0627215 0.00294751 0.998027 26.2605 0 0 0 1
32 0.99808 0.0063692 -0.0616159 -1.48954 -0.00671918 0.999962 -0.00547459 -0.302321 0.0615787 0.00587809 0.998085 27.2168 0 0 0 1
33 0.99813 0.00376787 -0.0610159 -1.54654 -0.00404632 0.999982 -0.0044408 -0.313516 0.0609981 0.00467938 0.998127 28.1829 0 0 0 1
34 0.998113 0.00193972 -0.0613743 -1.60668 -0.00191171 0.999998 0.000515183 -0.324411 0.0613752 -0.000396881 0.998115 29.1626 0 0 0 1
35 0.99806 -0.0017885 -0.062228 -1.66532 0.00203402 0.99999 0.00388232 -0.335656 0.0622204 -0.00400136 0.998054 30.1428 0 0 0 1
36 0.997945 -0.00917543 -0.0634115 -1.72059 0.00939451 0.999951 0.00315749 -0.343316 0.0633794 -0.00374672 0.997982 31.1244 0 0 0 1
37 0.997825 -0.0112684 -0.0649459 -1.78049 0.011242 0.999937 -0.000771312 -0.350864 0.0649504 3.95099e-05 0.997888 32.1064 0 0 0 1
38 0.997739 -0.0110126 -0.0662983 -1.85007 0.0107254 0.999932 -0.00468596 -0.361068 0.0663454 0.00396429 0.997789 33.0886 0 0 0 1
39 0.997597 -0.00959503 -0.0686163 -1.92119 0.00924037 0.999942 -0.00548426 -0.373466 0.0686649 0.00483704 0.997628 34.0774 0 0 0 1
40 0.99755 -0.0095802 -0.0693031 -1.99331 0.00931271 0.999948 -0.00418184 -0.387047 0.0693396 0.00352619 0.997587 35.0736 0 0 0 1
41 0.997473 -0.00634387 -0.0707596 -2.0707 0.00626661 0.99998 -0.0013139 -0.403858 0.0707665 0.00086716 0.997493 36.0721 0 0 0 1
42 0.99739 -0.00624366 -0.0719343 -2.14553 0.00625582 0.99998 -5.62375e-05 -0.416888 0.0719332 -0.000393917 0.997409 37.0728 0 0 0 1
43 0.997312 -0.00473093 -0.0731254 -2.21909 0.00492848 0.999985 0.00252135 -0.428625 0.0731123 -0.00287497 0.99732 38.0643 0 0 0 1
44 0.997318 -0.00467696 -0.0730348 -2.29215 0.00509473 0.999972 0.00553481 -0.440023 0.0730068 -0.00589206 0.997314 39.0618 0 0 0 1
45 0.997274 0.00138304 -0.0737801 -2.37574 -0.000811217 0.999969 0.00777971 -0.447869 0.0737886 -0.00769865 0.997244 40.0548 0 0 0 1
46 0.997262 0.00149131 -0.0739326 -2.45529 -0.000969511 0.999974 0.00709318 -0.454763 0.0739413 -0.00700208 0.997238 41.0557 0 0 0 1
47 0.997266 0.00175929 -0.0738699 -2.53081 -0.00136899 0.999985 0.00533379 -0.460519 0.0738782 -0.00521809 0.997254 42.0518 0 0 0 1
48 0.997253 0.00408494 -0.0739555 -2.61212 -0.00386552 0.999988 0.00310988 -0.469863 0.0739673 -0.00281546 0.997257 43.0493 0 0 0 1
49 0.997185 0.00365371 -0.0748884 -2.68799 -0.00342799 0.999989 0.00314243 -0.47951 0.0748991 -0.00287687 0.997187 44.0473 0 0 0 1
50 0.997077 0.00181435 -0.0763845 -2.76071 -0.00149292 0.99999 0.00426495 -0.487845 0.0763915 -0.00413845 0.997069 45.0403 0 0 0 1
51 0.997018 0.00246727 -0.0771352 -2.84117 -0.00206285 0.999984 0.00532227 -0.499132 0.0771471 -0.00514727 0.997006 46.0244 0 0 0 1
52 0.996991 0.00504805 -0.0773507 -2.92304 -0.00493379 0.999986 0.00166824 -0.510863 0.0773581 -0.00128158 0.997003 46.994 0 0 0 1
53 0.996911 0.00581773 -0.0783264 -3.00373 -0.00604061 0.999978 -0.00260888 -0.521193 0.0783095 0.00307396 0.996924 47.9551 0 0 0 1
54 0.996846 0.00678413 -0.0790757 -3.08343 -0.00711636 0.999967 -0.00392044 -0.534186 0.0790465 0.00447081 0.996861 48.9236 0 0 0 1
55 0.996843 0.00557268 -0.0792034 -3.16262 -0.00562268 0.999984 -0.000408328 -0.54901 0.0791999 0.000852374 0.996858 49.9005 0 0 0 1
56 0.996831 0.00375007 -0.0794568 -3.23868 -0.00354655 0.99999 0.00270227 -0.563036 0.0794661 -0.0024119 0.996835 50.8752 0 0 0 1
57 0.996805 0.00190455 -0.0798474 -3.31582 -0.00164885 0.999993 0.00326822 -0.574113 0.0798531 -0.00312612 0.996802 51.8394 0 0 0 1
58 0.996782 -0.00124932 -0.0801505 -3.39153 0.00141878 0.999997 0.0020573 -0.586659 0.0801477 -0.00216439 0.996781 52.8005 0 0 0 1
59 0.996745 -0.0038025 -0.0805262 -3.4676 0.0038689 0.999992 0.000668539 -0.59892 0.080523 -0.00097791 0.996752 53.7575 0 0 0 1
60 0.996643 -0.00519016 -0.0817059 -3.54489 0.00535256 0.999984 0.00176869 -0.60864 0.0816955 -0.00220009 0.996655 54.708 0 0 0 1
61 0.996534 -0.0079249 -0.0828082 -3.62139 0.00842977 0.999948 0.00574894 -0.618858 0.0827583 -0.00642707 0.996549 55.6588 0 0 0 1
62 0.996473 -0.00854289 -0.0834829 -3.69959 0.00945654 0.9999 0.0105549 -0.624401 0.0833844 -0.0113071 0.996453 56.6119 0 0 0 1
63 0.996447 -0.00664747 -0.083957 -3.78502 0.00773966 0.99989 0.0126902 -0.629769 0.0838633 -0.0132949 0.996389 57.5607 0 0 0 1
64 0.996335 -0.00522633 -0.0853755 -3.8689 0.00597793 0.999946 0.00855017 -0.636709 0.0853262 -0.0090292 0.996312 58.4941 0 0 0 1
65 0.996221 -0.00343661 -0.0867892 -3.95276 0.00350579 0.999994 0.000644619 -0.644008 0.0867865 -0.000946448 0.996226 59.4131 0 0 0 1
66 0.996144 -0.00149623 -0.0877201 -4.03806 0.00112725 0.99999 -0.00425562 -0.655271 0.0877256 0.00414033 0.996136 60.3236 0 0 0 1
67 0.996055 0.00375138 -0.0886573 -4.12895 -0.00406723 0.999986 -0.00338223 -0.671324 0.0886434 0.00372948 0.996056 61.2274 0 0 0 1
68 0.995922 0.00719305 -0.0899263 -4.21985 -0.0073202 0.999973 -0.00108421 -0.691307 0.089916 0.00173807 0.995948 62.125 0 0 0 1
69 0.99582 0.00967277 -0.0908194 -4.30702 -0.00966905 0.999953 0.000481019 -0.708494 0.0908198 0.000399128 0.995867 63.0134 0 0 0 1
70 0.995713 0.0102896 -0.0919182 -4.39131 -0.0103098 0.999947 0.000255248 -0.721276 0.091916 0.000693502 0.995767 63.8776 0 0 0 1
71 0.99554 0.0119225 -0.0935844 -4.477 -0.0118725 0.999929 0.00109156 -0.734766 0.0935908 2.43836e-05 0.995611 64.7307 0 0 0 1
72 0.995397 0.0126524 -0.0950024 -4.56121 -0.0125521 0.99992 0.00165348 -0.749039 0.0950157 -0.000453392 0.995476 65.5703 0 0 0 1
73 0.995256 0.0126635 -0.0964665 -4.64297 -0.0125254 0.999919 0.00203772 -0.761909 0.0964846 -0.00081977 0.995334 66.3938 0 0 0 1
74 0.995133 0.0127023 -0.0977168 -4.72623 -0.0124698 0.999918 0.00298947 -0.7711 0.0977468 -0.00175641 0.99521 67.2017 0 0 0 1
75 0.994948 0.015548 -0.0991814 -4.81287 -0.0150604 0.999871 0.00566291 -0.780301 0.0992566 -0.0041406 0.995053 67.9995 0 0 0 1
76 0.994794 0.0171065 -0.100462 -4.90076 -0.0162095 0.999821 0.009738 -0.788037 0.100611 -0.00805885 0.994893 68.7922 0 0 0 1
77 0.994568 0.0201446 -0.102122 -4.98771 -0.0190681 0.999752 0.0115061 -0.794955 0.102328 -0.00949629 0.994705 69.5653 0 0 0 1
78 0.99437 0.0229894 -0.10344 -5.07707 -0.0223435 0.999723 0.00739861 -0.804841 0.103581 -0.00504575 0.994608 70.3178 0 0 0 1
79 0.99423 0.0228747 -0.1048 -5.16223 -0.0229899 0.999736 0.000108822 -0.81531 0.104774 0.00230114 0.994493 71.0475 0 0 0 1
80 0.994077 0.0219938 -0.106431 -5.24094 -0.0227304 0.999725 -0.00571252 -0.825441 0.106276 0.00809789 0.994304 71.7584 0 0 0 1
81 0.994023 0.0228054 -0.106762 -5.32437 -0.0236929 0.999694 -0.00705161 -0.831667 0.106569 0.00953897 0.99426 72.4548 0 0 0 1
82 0.99386 0.0255543 -0.107653 -5.40648 -0.0260808 0.999654 -0.00348505 -0.846106 0.107527 0.00627133 0.994182 73.1337 0 0 0 1
83 0.993702 0.0257681 -0.109048 -5.48436 -0.02605 0.99966 -0.00116096 -0.865059 0.108981 0.00399435 0.994036 73.7942 0 0 0 1
84 0.99367 0.0225468 -0.110051 -5.5561 -0.0231761 0.999722 -0.00444219 -0.879535 0.10992 0.00696462 0.993916 74.4324 0 0 0 1
85 0.993802 0.0143509 -0.110234 -5.61528 -0.0155198 0.999832 -0.00975281 -0.89282 0.110075 0.0114032 0.993858 75.0504 0 0 0 1
86 0.993949 0.0102 -0.10937 -5.67796 -0.0118817 0.999821 -0.0147354 -0.904058 0.1092 0.0159457 0.993892 75.6535 0 0 0 1
87 0.994244 0.0126451 -0.106395 -5.74524 -0.014328 0.999784 -0.0150673 -0.916949 0.106181 0.016505 0.99421 76.2455 0 0 0 1
88 0.994592 0.0175824 -0.102356 -5.81375 -0.0189231 0.999747 -0.0121417 -0.930972 0.102117 0.0140129 0.994674 76.8222 0 0 0 1
89 0.995077 0.0159295 -0.0978149 -5.86699 -0.0169123 0.999814 -0.00922648 -0.942909 0.0976498 0.0108353 0.995162 77.3862 0 0 0 1
90 0.995665 0.0122046 -0.0922106 -5.90659 -0.0127358 0.999906 -0.00517412 -0.954487 0.0921387 0.00632606 0.995726 77.9355 0 0 0 1
91 0.996426 0.00781104 -0.084105 -5.94257 -0.00798227 0.999967 -0.0016998 -0.970792 0.0840889 0.00236507 0.996455 78.4692 0 0 0 1
92 0.997233 0.0114593 -0.0734453 -5.98049 -0.0116369 0.99993 -0.00198965 -0.981019 0.0734174 0.00283882 0.997297 78.9883 0 0 0 1
93 0.998165 0.0165636 -0.0582361 -6.0154 -0.0167456 0.999856 -0.0026387 -0.99003 0.058184 0.00360906 0.998299 79.4952 0 0 0 1
95 0.999635 0.0200255 -0.0181151 -6.02981 -0.0200623 0.999797 -0.00185529 -1.00502 0.0180742 0.00221804 0.999834 80.4727 0 0 0 1
97 0.999162 0.015548 0.037857 -5.96801 -0.0155684 0.999879 0.000243918 -1.02389 -0.0378486 -0.000833085 0.999283 81.4025 0 0 0 1
99 0.993959 0.0151454 0.108698 -5.84553 -0.0154328 0.999879 0.0018028 -1.04109 -0.108657 -0.00346942 0.994073 82.2952 0 0 0 1
101 0.980499 0.0151504 0.195937 -5.64466 -0.0157106 0.999876 0.00130478 -1.05761 -0.195893 -0.00435763 0.980616 83.1489 0 0 0 1
103 0.954186 0.0182833 0.298656 -5.36588 -0.0179595 0.999831 -0.00382887 -1.08348 -0.298675 -0.00171027 0.954353 83.9397 0 0 0 1
105 0.910736 0.0194893 0.412529 -4.99648 -0.0175815 0.99981 -0.00842014 -1.10057 -0.412615 0.000415655 0.910905 84.6633 0 0 0 1
107 0.848724 0.0183908 0.528517 -4.54701 -0.0135972 0.999824 -0.0129557 -1.12003 -0.528662 0.00380946 0.848824 85.2983 0 0 0 1
109 0.772259 0.0170098 0.63508 -4.0183 -0.0106749 0.999848 -0.0137989 -1.14244 -0.635218 0.00387688 0.772323 85.8601 0 0 0 1
111 0.684256 0.0156411 0.729074 -3.42903 -0.0102231 0.999877 -0.0118561 -1.16079 -0.72917 0.000659179 0.684332 86.3474 0 0 0 1
113 0.590745 0.011826 0.806772 -2.77931 -0.000173089 0.999894 -0.0145301 -1.17886 -0.806858 0.00844396 0.590684 86.7443 0 0 0 1
115 0.496173 0.0169181 0.868059 -2.10955 -0.00504039 0.999849 -0.0166057 -1.20339 -0.868209 0.00386392 0.496183 87.0847 0 0 0 1
117 0.408192 0.0165355 0.912746 -1.40862 0.00231553 0.999814 -0.0191484 -1.2249 -0.912893 0.00992974 0.408078 87.3396 0 0 0 1
119 0.333443 0.00543386 0.942754 -0.671521 0.0223493 0.999657 -0.0136666 -1.23662 -0.942505 0.025627 0.333208 87.522 0 0 0 1
121 0.269054 0.0173163 0.962969 0.0638526 0.00961829 0.99974 -0.0206648 -1.26307 -0.963077 0.0148221 0.268818 87.7199 0 0 0 1
123 0.214897 0.0233915 0.976357 0.843046 0.00677025 0.999653 -0.0254398 -1.30009 -0.976613 0.0120771 0.214664 87.8763 0 0 0 1
125 0.171479 0.031054 0.984698 1.66216 0.0212619 0.999154 -0.0352125 -1.3179 -0.984958 0.0269747 0.170674 87.9743 0 0 0 1
127 0.134011 0.0386308 0.990227 2.52547 0.0207141 0.998912 -0.041773 -1.34147 -0.990763 0.0261097 0.133065 88.0809 0 0 0 1
129 0.10418 0.0310179 0.994075 3.44652 0.0195614 0.999256 -0.0332297 -1.39013 -0.994366 0.0229074 0.103496 88.1692 0 0 0 1
131 0.0794366 0.027788 0.996453 4.42556 0.0261822 0.999208 -0.0299521 -1.42776 -0.996496 0.0284686 0.0786462 88.2295 0 0 0 1
132 0.0693462 0.028443 0.997187 4.93885 0.0294969 0.999098 -0.0305488 -1.44582 -0.997156 0.0315324 0.0684447 88.2553 0 0 0 1
133 0.0615414 0.0290168 0.997683 5.46907 0.0316982 0.999016 -0.0310108 -1.46406 -0.997601 0.0335332 0.0605611 88.2814 0 0 0 1
134 0.0559347 0.029371 0.998002 6.0151 0.0334765 0.99895 -0.0312751 -1.48373 -0.997873 0.035159 0.0548927 88.307 0 0 0 1
135 0.0504312 0.0304374 0.998264 6.58025 0.0349281 0.99887 -0.0322204 -1.50267 -0.998117 0.0364923 0.0493112 88.3306 0 0 0 1
136 0.0445067 0.0311103 0.998525 7.16082 0.0355578 0.998832 -0.0327048 -1.52353 -0.998376 0.0369609 0.0433485 88.3531 0 0 0 1
137 0.040243 0.0311989 0.998703 7.76375 0.0381603 0.998735 -0.0327376 -1.54487 -0.998461 0.0394283 0.0390016 88.3716 0 0 0 1
138 0.0373982 0.0312027 0.998813 8.38568 0.0397152 0.998676 -0.0326855 -1.56772 -0.998511 0.0408905 0.0361095 88.3901 0 0 0 1
139 0.0343726 0.0307634 0.998936 9.02449 0.0406913 0.998654 -0.0321549 -1.59059 -0.99858 0.0417533 0.0330745 88.4092 0 0 0 1
140 0.0320861 0.0302694 0.999027 9.68038 0.0427798 0.998584 -0.03163 -1.61442 -0.998569 0.043753 0.0307457 88.4263 0 0 0 1
141 0.0316452 0.0299561 0.99905 10.3542 0.0473602 0.998383 -0.0314363 -1.63856 -0.998376 0.04831 0.0301753 88.4381 0 0 0 1
142 0.0327723 0.029714 0.999021 11.0457 0.0507142 0.998221 -0.0313539 -1.66282 -0.998175 0.0516921 0.031207 88.4556 0 0 0 1
143 0.0353027 0.0297602 0.998933 11.7546 0.0522842 0.998133 -0.0315841 -1.68678 -0.998008 0.0533435 0.0336808 88.4781 0 0 0 1
144 0.0392372 0.0297502 0.998787 12.4771 0.0547241 0.997993 -0.0318763 -1.71289 -0.99773 0.0559084 0.0375304 88.5062 0 0 0 1
145 0.0437096 0.0293188 0.998614 13.219 0.0550685 0.997979 -0.0317105 -1.73922 -0.997525 0.0563782 0.0420067 88.5387 0 0 0 1
146 0.0477725 0.0278103 0.998471 13.9764 0.0564652 0.997939 -0.0304971 -1.76499 -0.997261 0.0578358 0.0461037 88.5751 0 0 0 1
147 0.0518486 0.0263145 0.998308 14.7472 0.0562418 0.997989 -0.0292271 -1.79222 -0.99707 0.057662 0.0502644 88.6178 0 0 0 1
148 0.0560658 0.0242863 0.998132 15.5313 0.0531494 0.998214 -0.0272738 -1.82056 -0.997011 0.0545792 0.0546748 88.6693 0 0 0 1
149 0.0600218 0.0233355 0.997924 16.3271 0.0522059 0.998285 -0.0264839 -1.84733 -0.996831 0.0536871 0.0587006 88.7243 0 0 0 1
150 0.0641513 0.0243795 0.997642 17.1258 0.0492204 0.998408 -0.0275632 -1.87761 -0.996726 0.0508726 0.0628492 88.7821 0 0 0 1
151 0.0672583 0.028483 0.997329 17.929 0.0470717 0.998389 -0.0316877 -1.91204 -0.996625 0.0490772 0.0658092 88.842 0 0 0 1
152 0.0688453 0.0337446 0.997056 18.7357 0.0413971 0.99847 -0.0366509 -1.9468 -0.996768 0.0437985 0.067343 88.9041 0 0 0 1
153 0.0686545 0.0370247 0.996953 19.5482 0.0387033 0.99846 -0.0397459 -1.98038 -0.996889 0.0413142 0.0671158 88.9665 0 0 0 1

77
examples/Data/VO_camera_poses00s.txt Normal file
View File

@ -0,0 +1,77 @@
0 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
1 0.99999 -0.00268679 -0.00354618 6.43221e-05 0.00267957 0.999994 -0.00204036 -0.0073023 0.00355164 0.00203084 0.999992 0.676456 0 0 0 1
2 0.999969 -0.00120771 -0.00772489 -0.0100328 0.00117985 0.999993 -0.003611 -0.0111185 0.00772919 0.00360178 0.999964 1.37125 0 0 0 1
3 0.999931 -0.00128098 -0.0117006 -0.0237327 0.00122052 0.999986 -0.00517227 -0.0136538 0.0117071 0.00515763 0.999918 2.08563 0 0 0 1
4 0.99986 5.79321e-05 -0.0167106 -0.0402272 -0.000155312 0.999983 -0.00582618 -0.0194327 0.01671 0.00582796 0.999843 2.81528 0 0 0 1
5 0.999772 -0.00118366 -0.0213077 -0.0572378 0.0010545 0.999981 -0.00607208 -0.0278191 0.0213145 0.00604822 0.999755 3.56204 0 0 0 1
6 0.999662 0.000544425 -0.0259946 -0.081545 -0.000735472 0.999973 -0.00734051 -0.0358844 0.0259899 0.00735714 0.999635 4.32265 0 0 0 1
7 0.999513 0.0032602 -0.0310324 -0.112137 -0.0035101 0.999962 -0.00800188 -0.0447209 0.0310051 0.00810691 0.999486 5.09668 0 0 0 1
8 0.999361 0.00349173 -0.0355658 -0.143594 -0.00372162 0.999973 -0.00639979 -0.0532611 0.0355425 0.00652807 0.999347 5.88701 0 0 0 1
9 0.999185 0.00268131 -0.040271 -0.176401 -0.0028332 0.999989 -0.00371493 -0.0632884 0.0402606 0.003826 0.999182 6.6897 0 0 0 1
10 0.99903 0.00226305 -0.0439747 -0.211687 -0.00231163 0.999997 -0.00105382 -0.072362 0.0439722 0.00115445 0.999032 7.50361 0 0 0 1
11 0.998896 0.00366482 -0.0468376 -0.254125 -0.00374515 0.999992 -0.00162734 -0.0820263 0.0468312 0.00180096 0.998901 8.32333 0 0 0 1
12 0.998775 0.00304285 -0.0493866 -0.295424 -0.00313866 0.999993 -0.00186268 -0.0885739 0.0493806 0.00201541 0.998778 9.15211 0 0 0 1
13 0.998682 7.09894e-05 -0.0513155 -0.334647 -0.000203775 0.999997 -0.00258241 -0.0938889 0.0513152 0.00258946 0.998679 9.98839 0 0 0 1
14 0.998565 -8.82523e-05 -0.0535542 -0.380835 -9.36659e-06 0.999998 -0.00182255 -0.10173 0.0535542 0.00182044 0.998563 10.832 0 0 0 1
15 0.998481 -0.00146793 -0.0550718 -0.429135 0.0013525 0.999997 -0.00213307 -0.111427 0.0550748 0.00205535 0.99848 11.687 0 0 0 1
16 0.998373 0.000738731 -0.0570218 -0.483426 -0.000993083 0.99999 -0.00443241 -0.122139 0.0570179 0.00448183 0.998363 12.5483 0 0 0 1
17 0.998285 0.00120595 -0.0585258 -0.540056 -0.00162301 0.999974 -0.00707907 -0.132598 0.0585158 0.00716191 0.998261 13.4179 0 0 0 1
18 0.998165 0.00516151 -0.060337 -0.6023 -0.00570195 0.999945 -0.00878826 -0.143753 0.0602883 0.00911617 0.998139 14.2952 0 0 0 1
19 0.998101 0.00610094 -0.0612993 -0.66308 -0.00663017 0.999942 -0.00843386 -0.157854 0.0612443 0.00882427 0.998084 15.1802 0 0 0 1
20 0.998014 0.0052997 -0.0627662 -0.722045 -0.00574767 0.999959 -0.0069587 -0.172847 0.0627268 0.00730564 0.998004 16.074 0 0 0 1
21 0.99792 0.00591748 -0.0641975 -0.78346 -0.00627924 0.999966 -0.00543487 -0.186221 0.0641631 0.00582667 0.997922 16.9738 0 0 0 1
22 0.997857 0.00547694 -0.0651993 -0.845347 -0.00584101 0.999968 -0.00539455 -0.199741 0.0651677 0.00576382 0.997858 17.8786 0 0 0 1
23 0.997737 0.00536917 -0.0670282 -0.908218 -0.00579979 0.999964 -0.0062316 -0.212775 0.0669924 0.00660624 0.997732 18.7877 0 0 0 1
24 0.997663 0.00386695 -0.0682185 -0.971291 -0.00435203 0.999966 -0.00696344 -0.226442 0.0681893 0.00724406 0.997646 19.7046 0 0 0 1
25 0.997629 0.00410637 -0.0687004 -1.03663 -0.00448288 0.999976 -0.00532714 -0.239555 0.0686769 0.00562249 0.997623 20.6257 0 0 0 1
26 0.997617 0.00588773 -0.0687501 -1.10557 -0.0060349 0.99998 -0.00193325 -0.254273 0.0687373 0.00234355 0.997632 21.55 0 0 0 1
27 0.997662 0.00693766 -0.0679906 -1.17297 -0.00682806 0.999975 0.0018442 -0.26563 0.0680017 -0.00137565 0.997684 22.4875 0 0 0 1
28 0.997774 0.00579785 -0.0664343 -1.23728 -0.00550265 0.999974 0.00462554 -0.271962 0.0664594 -0.00424968 0.99778 23.4285 0 0 0 1
29 0.997872 0.00589563 -0.0649408 -1.30214 -0.00556012 0.99997 0.00534586 -0.277922 0.0649704 -0.0049734 0.997875 24.3732 0 0 0 1
30 0.997958 0.00627024 -0.0635595 -1.36462 -0.00612984 0.999978 0.00240374 -0.285335 0.0635732 -0.00200922 0.997975 25.314 0 0 0 1
31 0.998004 0.00714074 -0.0627411 -1.42783 -0.00731158 0.99997 -0.00249375 -0.293171 0.0627215 0.00294751 0.998027 26.2605 0 0 0 1
32 0.99808 0.0063692 -0.0616159 -1.48954 -0.00671918 0.999962 -0.00547459 -0.302321 0.0615787 0.00587809 0.998085 27.2168 0 0 0 1
33 0.99813 0.00376787 -0.0610159 -1.54654 -0.00404632 0.999982 -0.0044408 -0.313516 0.0609981 0.00467938 0.998127 28.1829 0 0 0 1
34 0.998113 0.00193972 -0.0613743 -1.60668 -0.00191171 0.999998 0.000515183 -0.324411 0.0613752 -0.000396881 0.998115 29.1626 0 0 0 1
35 0.99806 -0.0017885 -0.062228 -1.66532 0.00203402 0.99999 0.00388232 -0.335656 0.0622204 -0.00400136 0.998054 30.1428 0 0 0 1
36 0.997945 -0.00917543 -0.0634115 -1.72059 0.00939451 0.999951 0.00315749 -0.343316 0.0633794 -0.00374672 0.997982 31.1244 0 0 0 1
37 0.997825 -0.0112684 -0.0649459 -1.78049 0.011242 0.999937 -0.000771312 -0.350864 0.0649504 3.95099e-05 0.997888 32.1064 0 0 0 1
38 0.997739 -0.0110126 -0.0662983 -1.85007 0.0107254 0.999932 -0.00468596 -0.361068 0.0663454 0.00396429 0.997789 33.0886 0 0 0 1
39 0.997597 -0.00959503 -0.0686163 -1.92119 0.00924037 0.999942 -0.00548426 -0.373466 0.0686649 0.00483704 0.997628 34.0774 0 0 0 1
40 0.99755 -0.0095802 -0.0693031 -1.99331 0.00931271 0.999948 -0.00418184 -0.387047 0.0693396 0.00352619 0.997587 35.0736 0 0 0 1
41 0.997473 -0.00634387 -0.0707596 -2.0707 0.00626661 0.99998 -0.0013139 -0.403858 0.0707665 0.00086716 0.997493 36.0721 0 0 0 1
42 0.99739 -0.00624366 -0.0719343 -2.14553 0.00625582 0.99998 -5.62375e-05 -0.416888 0.0719332 -0.000393917 0.997409 37.0728 0 0 0 1
43 0.997312 -0.00473093 -0.0731254 -2.21909 0.00492848 0.999985 0.00252135 -0.428625 0.0731123 -0.00287497 0.99732 38.0643 0 0 0 1
44 0.997318 -0.00467696 -0.0730348 -2.29215 0.00509473 0.999972 0.00553481 -0.440023 0.0730068 -0.00589206 0.997314 39.0618 0 0 0 1
45 0.997274 0.00138304 -0.0737801 -2.37574 -0.000811217 0.999969 0.00777971 -0.447869 0.0737886 -0.00769865 0.997244 40.0548 0 0 0 1
46 0.997262 0.00149131 -0.0739326 -2.45529 -0.000969511 0.999974 0.00709318 -0.454763 0.0739413 -0.00700208 0.997238 41.0557 0 0 0 1
47 0.997266 0.00175929 -0.0738699 -2.53081 -0.00136899 0.999985 0.00533379 -0.460519 0.0738782 -0.00521809 0.997254 42.0518 0 0 0 1
48 0.997253 0.00408494 -0.0739555 -2.61212 -0.00386552 0.999988 0.00310988 -0.469863 0.0739673 -0.00281546 0.997257 43.0493 0 0 0 1
49 0.997185 0.00365371 -0.0748884 -2.68799 -0.00342799 0.999989 0.00314243 -0.47951 0.0748991 -0.00287687 0.997187 44.0473 0 0 0 1
50 0.997077 0.00181435 -0.0763845 -2.76071 -0.00149292 0.99999 0.00426495 -0.487845 0.0763915 -0.00413845 0.997069 45.0403 0 0 0 1
51 0.997018 0.00246727 -0.0771352 -2.84117 -0.00206285 0.999984 0.00532227 -0.499132 0.0771471 -0.00514727 0.997006 46.0244 0 0 0 1
52 0.996991 0.00504805 -0.0773507 -2.92304 -0.00493379 0.999986 0.00166824 -0.510863 0.0773581 -0.00128158 0.997003 46.994 0 0 0 1
53 0.996911 0.00581773 -0.0783264 -3.00373 -0.00604061 0.999978 -0.00260888 -0.521193 0.0783095 0.00307396 0.996924 47.9551 0 0 0 1
54 0.996846 0.00678413 -0.0790757 -3.08343 -0.00711636 0.999967 -0.00392044 -0.534186 0.0790465 0.00447081 0.996861 48.9236 0 0 0 1
55 0.996843 0.00557268 -0.0792034 -3.16262 -0.00562268 0.999984 -0.000408328 -0.54901 0.0791999 0.000852374 0.996858 49.9005 0 0 0 1
56 0.996831 0.00375007 -0.0794568 -3.23868 -0.00354655 0.99999 0.00270227 -0.563036 0.0794661 -0.0024119 0.996835 50.8752 0 0 0 1
57 0.996805 0.00190455 -0.0798474 -3.31582 -0.00164885 0.999993 0.00326822 -0.574113 0.0798531 -0.00312612 0.996802 51.8394 0 0 0 1
58 0.996782 -0.00124932 -0.0801505 -3.39153 0.00141878 0.999997 0.0020573 -0.586659 0.0801477 -0.00216439 0.996781 52.8005 0 0 0 1
59 0.996745 -0.0038025 -0.0805262 -3.4676 0.0038689 0.999992 0.000668539 -0.59892 0.080523 -0.00097791 0.996752 53.7575 0 0 0 1
60 0.996643 -0.00519016 -0.0817059 -3.54489 0.00535256 0.999984 0.00176869 -0.60864 0.0816955 -0.00220009 0.996655 54.708 0 0 0 1
61 0.996534 -0.0079249 -0.0828082 -3.62139 0.00842977 0.999948 0.00574894 -0.618858 0.0827583 -0.00642707 0.996549 55.6588 0 0 0 1
62 0.996473 -0.00854289 -0.0834829 -3.69959 0.00945654 0.9999 0.0105549 -0.624401 0.0833844 -0.0113071 0.996453 56.6119 0 0 0 1
63 0.996447 -0.00664747 -0.083957 -3.78502 0.00773966 0.99989 0.0126902 -0.629769 0.0838633 -0.0132949 0.996389 57.5607 0 0 0 1
64 0.996335 -0.00522633 -0.0853755 -3.8689 0.00597793 0.999946 0.00855017 -0.636709 0.0853262 -0.0090292 0.996312 58.4941 0 0 0 1
65 0.996221 -0.00343661 -0.0867892 -3.95276 0.00350579 0.999994 0.000644619 -0.644008 0.0867865 -0.000946448 0.996226 59.4131 0 0 0 1
66 0.996144 -0.00149623 -0.0877201 -4.03806 0.00112725 0.99999 -0.00425562 -0.655271 0.0877256 0.00414033 0.996136 60.3236 0 0 0 1
67 0.996055 0.00375138 -0.0886573 -4.12895 -0.00406723 0.999986 -0.00338223 -0.671324 0.0886434 0.00372948 0.996056 61.2274 0 0 0 1
68 0.995922 0.00719305 -0.0899263 -4.21985 -0.0073202 0.999973 -0.00108421 -0.691307 0.089916 0.00173807 0.995948 62.125 0 0 0 1
69 0.99582 0.00967277 -0.0908194 -4.30702 -0.00966905 0.999953 0.000481019 -0.708494 0.0908198 0.000399128 0.995867 63.0134 0 0 0 1
70 0.995713 0.0102896 -0.0919182 -4.39131 -0.0103098 0.999947 0.000255248 -0.721276 0.091916 0.000693502 0.995767 63.8776 0 0 0 1
71 0.99554 0.0119225 -0.0935844 -4.477 -0.0118725 0.999929 0.00109156 -0.734766 0.0935908 2.43836e-05 0.995611 64.7307 0 0 0 1
72 0.995397 0.0126524 -0.0950024 -4.56121 -0.0125521 0.99992 0.00165348 -0.749039 0.0950157 -0.000453392 0.995476 65.5703 0 0 0 1
73 0.995256 0.0126635 -0.0964665 -4.64297 -0.0125254 0.999919 0.00203772 -0.761909 0.0964846 -0.00081977 0.995334 66.3938 0 0 0 1
74 0.995133 0.0127023 -0.0977168 -4.72623 -0.0124698 0.999918 0.00298947 -0.7711 0.0977468 -0.00175641 0.99521 67.2017 0 0 0 1
75 0.994948 0.015548 -0.0991814 -4.81287 -0.0150604 0.999871 0.00566291 -0.780301 0.0992566 -0.0041406 0.995053 67.9995 0 0 0 1
76 0.994794 0.0171065 -0.100462 -4.90076 -0.0162095 0.999821 0.009738 -0.788037 0.100611 -0.00805885 0.994893 68.7922 0 0 0 1

88781
examples/Data/VO_stereo_factors00.txt Normal file
View File

File diff suppressed because it is too large Load Diff

52544
examples/Data/VO_stereo_factors00s.txt Normal file
View File

File diff suppressed because it is too large Load Diff

17
examples/Data/dubrovnik-1-1-pre.txt Normal file
View File

@ -0,0 +1,17 @@
1 1 1
0 0 -3.859900e+02 3.871200e+02
-1.6943983532198115e-02
1.1171804676513932e-02
2.4643508831711991e-03
7.3030995682610689e-01
-2.6490818471043420e-01
-1.7127892627337182e+00
1.4300319432711681e+03
-7.5572758535864072e-08
3.2377569465570913e-14
-1.2055995050700867e+01
1.2838775976205760e+01
-4.1099369264082803e+01

79
examples/Data/dubrovnik-3-7-18-pre.txt Normal file
View File

@ -0,0 +1,79 @@
3 7 18
0 0 -3.859900e+02 3.871200e+02
1 0 -3.844000e+01 4.921200e+02
2 0 -6.679200e+02 1.231100e+02
0 1 3.838800e+02 -1.529999e+01
1 1 5.597500e+02 -1.061500e+02
0 2 5.915500e+02 1.364400e+02
1 2 8.638600e+02 -2.346997e+01
2 2 4.947200e+02 1.125200e+02
0 3 5.925000e+02 1.257500e+02
1 3 8.610800e+02 -3.521997e+01
2 3 4.985400e+02 1.015600e+02
0 4 3.487200e+02 5.583800e+02
1 4 7.760300e+02 4.835300e+02
2 4 7.780029e+00 3.263500e+02
0 5 1.401001e+01 9.642001e+01
1 5 2.071300e+02 1.183600e+02
1 6 5.431801e+02 2.948100e+02
2 6 -5.841998e+01 1.108300e+02
-1.6943983532198115e-02
1.1171804676513932e-02
2.4643508831711991e-03
7.3030995682610689e-01
-2.6490818471043420e-01
-1.7127892627337182e+00
1.4300319432711681e+03
-7.5572758535864072e-08
3.2377569465570913e-14
1.5049725341485708e-02
-1.8504564785154357e-01
-2.9278402790141456e-01
-1.0590476152349551e+00
-3.6017862414345798e-02
-1.5720340175803784e+00
1.4321374541298685e+03
-7.3171919892612292e-08
3.1759419019880947e-14
-3.0793597986873011e-01
3.2077907982952031e-01
2.2253985096991455e-01
8.5034483295909009e+00
6.7499603629668741e+00
-3.6383814384447088e+00
1.5720470590375264e+03
-1.5962623661947355e-08
-1.6507904848058800e-14
-1.2055995050700867e+01
1.2838775976205760e+01
-4.1099369264082803e+01
6.4168905904672933e+00
3.8897031177598462e-01
-2.3586282709150449e+01
1.3051100355717297e+01
3.8387587111611952e+00
-2.9777932175344951e+01
1.3060946673472820e+01
3.5910521225905803e+00
-2.9759080795372942e+01
1.4265764475421857e+01
2.4096216156436530e+01
-5.4823971067225500e+01
-2.5292283211391348e-01
2.2166082122808284e+00
-2.1712127480255084e+01
7.6465738085189585e+00
1.4185331909846619e+01
-5.2070299568846060e+01

80
examples/Data/dubrovnik-3-7-pre-rewritten.txt
View File

@ -1,80 +0,0 @@
3 7 19
0 0 -385.989990234375 387.1199951171875
1 0 -38.439998626708984375 492.1199951171875
2 0 -667.91998291015625 123.1100006103515625
0 1 383.8800048828125 -15.299989700317382812
1 1 559.75 -106.15000152587890625
0 2 591.54998779296875 136.44000244140625
1 2 863.8599853515625 -23.469970703125
2 2 494.720001220703125 112.51999664306640625
0 3 592.5 125.75
1 3 861.08001708984375 -35.219970703125
2 3 498.540008544921875 101.55999755859375
0 4 348.720001220703125 558.3800048828125
1 4 776.030029296875 483.529998779296875
2 4 7.7800288200378417969 326.350006103515625
0 5 14.010009765625 96.420013427734375
1 5 207.1300048828125 118.3600006103515625
0 6 202.7599945068359375 340.989990234375
1 6 543.18011474609375 294.80999755859375
2 6 -58.419979095458984375 110.8300018310546875
0.29656188120312942935
-0.035318354384285870207
0.31252101755032046793
0.47230274932665988752
-0.3572340863744113415
-2.0517704282499575896
1430.031982421875
-7.5572756941255647689e-08
3.2377570134516087119e-14
0.28532097381985194184
-0.27699838370789808817
0.048601169984112867206
-1.2598695987143850861
-0.049063798188844320869
-1.9586867140445654023
1432.137451171875
-7.3171918302250560373e-08
3.1759419042137054801e-14
0.057491325683772541433
0.34853090049579965592
0.47985129303736057116
8.1963904289063389541
6.5146840788718787252
-3.8392804395897406344
1572.047119140625
-1.5962623223231275915e-08
-1.6507904730136101212e-14
-11.317351620610928364
3.3594874875767186673
-42.755222607849105998
4.2648515634753199066
-8.4629358700849355301
-22.252086323427270997
10.996977688149536689
-9.2123370180278048025
-29.206739014051372294
10.935342607054865383
-9.4338917557810741954
-29.112263909175499776
15.714024935401759819
1.3745079651566265433
-59.286834979937104606
-1.3624227800805182031
-4.1979357415396094666
-21.034430148188398846
6.7690173115899296974
-4.7352452433700786827
-53.605307875695892506

9
examples/Data/noisyToyGraph.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 0.774115 1.183389 1.576173
VERTEX_SE2 2 -0.262420 2.047059 -3.127594
VERTEX_SE2 3 -1.605649 0.993891 -1.561134
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

9
examples/Data/optimizedNoisyToyGraph.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE2 0 0.000000 -0.000000 0.000000
VERTEX_SE2 1 0.955797 1.137643 1.543041
VERTEX_SE2 2 0.129867 1.989651 3.201259
VERTEX_SE2 3 -1.047715 0.933789 4.743682
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

9
examples/Data/orientationsNoisyToyGraph.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 0.000000 0.000000 1.565449
VERTEX_SE2 2 0.000000 0.000000 3.134143
VERTEX_SE2 3 0.000000 0.000000 4.710123
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

23
examples/Data/pose2example.txt Normal file
View File

@ -0,0 +1,23 @@
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 1.030390 0.011350 -0.081596
VERTEX_SE2 2 2.036137 -0.129733 -0.301887
VERTEX_SE2 3 3.015097 -0.442395 -0.345514
VERTEX_SE2 4 3.343949 0.506678 1.214715
VERTEX_SE2 5 3.684491 1.464049 1.183785
VERTEX_SE2 6 4.064626 2.414783 1.176333
VERTEX_SE2 7 4.429778 3.300180 1.259169
VERTEX_SE2 8 4.128877 2.321481 -1.825391
VERTEX_SE2 9 3.884653 1.327509 -1.953016
VERTEX_SE2 10 3.531067 0.388263 -2.148934
EDGE_SE2 0 1 1.030390 0.011350 -0.081596 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 1 2 1.013900 -0.058639 -0.220291 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 2 3 1.027650 -0.007456 -0.043627 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 3 4 -0.012016 1.004360 1.560229 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 4 5 1.016030 0.014565 -0.030930 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 5 6 1.023890 0.006808 -0.007452 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 7 8 -1.023820 -0.013668 -3.084560 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 8 9 1.023440 0.013984 -0.127624 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 9 10 1.003350 0.022250 -0.195918 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 5 9 0.033943 0.032439 3.073637 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 3 10 0.044020 0.988477 -1.553511 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699

71
examples/Data/pose3example-grid.txt Normal file
View File

@ -0,0 +1,71 @@
VERTEX_SE3:QUAT 0 1.63791e-12 7.56548e-14 -3.02811e-12 5.35657e-13 2.43616e-13 9.71152e-14 1
VERTEX_SE3:QUAT 1 1.01609 0.00274307 -0.0351514 -0.499545 0.247735 0.723569 -0.406854
VERTEX_SE3:QUAT 2 1.99996 0.0304956 -0.040662 0.403501 -0.294714 -0.4254 0.754563
VERTEX_SE3:QUAT 3 1.94371 1.06535 0.0118614 -0.0471731 -0.541615 0.820893 0.17482
VERTEX_SE3:QUAT 4 0.962753 0.999477 0.0211017 -0.19663 -0.66009 0.470743 0.551379
VERTEX_SE3:QUAT 5 -0.00956768 0.965396 -0.021854 -0.320221 -0.518368 0.47521 0.634766
VERTEX_SE3:QUAT 6 -0.0863793 1.97682 0.000531117 -0.0173439 -0.573793 -0.450627 0.683663
VERTEX_SE3:QUAT 7 0.918905 2.01556 -0.0139773 0.56169 -0.440513 0.199057 0.671438
VERTEX_SE3:QUAT 8 1.92094 2.05524 0.0469884 0.0073084 -0.372357 -0.467582 0.801663
VERTEX_SE3:QUAT 9 1.86182 2.05449 1.09237 0.0131731 -0.05784 0.0335652 0.997674
VERTEX_SE3:QUAT 10 0.880176 2.02406 1.00997 -0.39342 -0.287909 0.757918 0.433462
VERTEX_SE3:QUAT 11 -0.0960463 1.98653 0.995791 0.434103 -0.199044 0.585176 0.655367
VERTEX_SE3:QUAT 12 -0.0911401 0.997117 0.988217 -0.0925477 0.572872 0.537294 0.612019
VERTEX_SE3:QUAT 13 0.948316 1.02239 0.991745 0.142484 0.560062 0.750078 0.321578
VERTEX_SE3:QUAT 14 1.92631 1.08945 1.06749 0.23878 0.380837 0.796564 -0.404269
VERTEX_SE3:QUAT 15 1.95398 0.0777667 0.982353 -0.384392 0.58733 0.685207 -0.194366
VERTEX_SE3:QUAT 16 0.946032 0.0482667 0.952308 -0.218979 0.186315 -0.494185 0.820437
VERTEX_SE3:QUAT 17 -0.0625076 -0.034424 0.942171 0.514725 -0.185043 -0.44771 0.707371
VERTEX_SE3:QUAT 18 -0.083807 -0.0106666 1.9853 0.00792651 1.98919e-05 -0.00128106 0.999968
VERTEX_SE3:QUAT 19 0.918067 -0.000897795 1.92157 -0.342141 0.241241 -0.726975 0.544288
VERTEX_SE3:QUAT 20 1.90041 0.0323631 2.00636 0.412572 -0.0930131 -0.133075 0.896339
VERTEX_SE3:QUAT 21 1.84895 1.05013 2.0738 -0.580757 0.35427 0.729393 -0.0721062
VERTEX_SE3:QUAT 22 0.880221 1.00671 1.99021 0.147752 0.355662 0.917953 0.095058
VERTEX_SE3:QUAT 23 -0.0950872 1.00374 1.95013 -0.29909 -0.0578461 0.857019 0.415594
VERTEX_SE3:QUAT 24 -0.111581 1.97979 1.98762 0.565153 0.214463 -0.523058 0.600848
VERTEX_SE3:QUAT 25 0.837568 2.01589 2.03075 -0.284756 0.369992 0.875484 -0.124692
VERTEX_SE3:QUAT 26 1.82708 2.05081 2.07052 0.254696 0.250865 0.653216 0.667462
EDGE_SE3:QUAT 0 1 1.00497 0.002077 -0.015539 -0.508004 0.250433 0.711222 -0.416386 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 1 2 -0.200593 0.339956 -0.908079 -0.093598 0.151993 0.42829 0.885836 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 2 3 -0.922791 0.330629 -0.292682 0.365657 -0.051986 0.924849 -0.090813 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 3 4 0.893075 0.246476 0.331154 -0.285927 0.341221 -0.267609 0.854517 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 4 5 0.280674 0.244242 0.923726 0.035064 0.21101 0.083834 0.973251 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 5 6 0.955621 0.355669 -0.025152 -0.306713 0.131221 -0.781587 0.527096 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 6 7 -0.076631 0.636081 -0.771439 0.702021 0.326514 0.122181 0.620988 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 7 8 0.582761 -0.721177 -0.376875 -0.733841 -0.170725 -0.256653 0.605359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 8 9 0.600312 0.298765 0.767014 0.057612 0.332574 0.486324 0.805956 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 9 10 -0.986649 0.03008 -0.008766 -0.362177 -0.253215 0.763748 0.470531 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 10 11 0.275109 0.534769 0.823463 0.450708 -0.472399 -0.432689 0.621677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 11 12 -0.61882 0.024878 0.773748 0.0927029 0.786162 -0.21122 0.573359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 12 13 -0.175537 -0.730832 0.634529 -0.018628 0.006375 0.428306 0.903419 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 13 14 -0.700208 -0.245198 0.637353 -0.035865 0.273394 0.645363 0.712374 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 14 15 0.373495 0.373768 -0.846199 0.400323 0.310362 -0.422222 0.751762 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 15 16 0.648588 0.157829 0.72252 0.781502 -0.210141 -0.501005 -0.30674 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 16 17 -0.390339 -0.702656 -0.572321 0.765815 0.055816 0.032478 0.63981 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 17 18 -0.261114 0.908685 0.421318 -0.501833 0.166567 0.448468 0.720622 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 18 19 1.00815 0.012634 -0.029822 -0.347007 0.205082 -0.740641 0.537569 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 19 20 -0.162376 0.581623 0.810804 0.628338 0.075411 0.650639 0.41973 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 20 21 -0.358942 0.627689 -0.704045 -0.469133 0.542456 0.530583 -0.451816 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 21 22 0.362417 0.298352 0.854822 0.004058 -0.696926 0.140345 0.703265 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 22 23 0.934942 0.020321 -0.358044 -0.445461 0.260916 -0.379862 0.767589 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 23 24 0.741887 -0.657659 0.215293 -0.584859 0.196138 0.688031 0.38221 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 24 25 0.300145 0.82011 -0.39974 0.46538 -0.593595 -0.202131 0.624668 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 25 26 -0.85591 0.022701 -0.510794 0.12929 -0.685192 -0.503707 0.509978 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 0 5 0.026721 0.990497 -0.007651 -0.317476 -0.510239 0.467341 0.648427 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 3 8 0.390516 -0.401461 -0.830724 0.503106 -0.367814 0.780584 0.047806 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 4 1 -0.813838 -0.446181 0.319175 0.224903 -0.031827 0.97265 0.048561 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 4 13 0.571273 -0.805401 0.077339 0.892031 0.329761 0.275468 0.140201 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 5 12 0.389794 -0.882655 0.268063 0.712423 0.550662 0.275339 0.33677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 6 11 0.800298 0.505022 0.361738 0.739335 0.419366 0.443817 0.283801 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 10 13 -0.912531 0.430955 -0.018942 0.830493 -0.093519 0.272041 0.477001 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 12 23 -0.797606 0.437737 0.311476 -0.657137 -0.196625 0.136652 0.714728 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 13 22 -0.116836 0.952032 0.269398 -0.216437 0.086571 0.260965 0.936781 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 14 21 0.749295 0.373389 0.581641 0.253048 0.511007 -0.537262 0.621439 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 16 1 0.160985 0.555966 -0.811911 0.748057 0.122381 -0.369631 0.537407 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 18 23 0.028909 1.02689 -0.00265 -0.294167 -0.071607 0.850901 0.429308 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 19 16 -0.230711 0.750637 -0.607511 0.14647 -0.102538 0.297899 0.937704 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 20 15 -0.031986 -0.741129 -0.728721 -0.278926 0.731172 0.404675 -0.473103 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 22 19 -0.332601 0.704401 -0.687251 -0.372165 -0.054346 0.713024 0.591725 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 22 25 0.347067 -0.634646 0.657147 0.018567 0.476762 0.040939 0.877882 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 25 10 0.388971 -0.723981 -0.559653 -0.373459 -0.014654 -0.696123 0.612965 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 26 21 -0.979482 -0.024822 0.043763 -0.326753 0.819942 0.292615 0.367837 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400

3
examples/Data/pose3example-offdiagonal-rewritten.txt Normal file
View File

@ -0,0 +1,3 @@
VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 1 1 1 1 1 10000 2 2 2 2 10000 3 3 3 10000 4 4 10000 5 10000

3
examples/Data/pose3example-offdiagonal.txt Normal file
View File

@ -0,0 +1,3 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000 10000.000000 2.000000 2.000000 2.000000 2.000000 10000.000000 3.000000 3.000000 3.000000 10000.000000 4.000000 4.000000 10000.000000 5.000000 10000.0000

11
examples/Data/pose3example.txt Normal file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.303380 -0.513226 0.542221 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.125250 -0.534379 0.769122 0.327419 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000

11
examples/Data/simpleGraph10gradIter.txt Normal file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.0008187 0.0011723 0.0895466 0.9959816
VERTEX_SE3:QUAT 1 0.000000 -0.000000 0.000000 0.0010673 0.0015636 0.1606931 0.9870026
VERTEX_SE3:QUAT 2 -0.388822 0.632954 0.001223 0.0029920 0.0014066 0.0258235 0.9996610
VERTEX_SE3:QUAT 3 -1.143204 0.050638 0.006026 -0.0012800 -0.0002767 -0.2850291 0.9585180
VERTEX_SE3:QUAT 4 -0.512416 0.486441 0.005171 0.0002681 0.0023574 0.0171476 0.9998502
EDGE_SE3:QUAT 1 2 1.000000 2.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 2 3 -0.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 3 4 1.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 3 1 0.000001 2.000000 0.000000 0.0000000 0.0000000 1.0000000 0.0000002 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 1 4 -1.000000 1.000000 0.000000 0.0000000 0.0000000 -0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 0 1 0.000000 0.000000 0.000000 0.0000000 0.0000000 0.0000000 1.0000000 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000

18
examples/LocalizationExample.cpp
View File

@ -93,8 +93,8 @@ public:
// Consequently, the Jacobians are:
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0);
return (Vector(2) << q.x() - mx_, q.y() - my_);
if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
// The second is a 'clone' function that allows the factor to be copied. Under most
@ -118,17 +118,17 @@ int main(int argc, char** argv) {
// 2a. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
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, 0.0), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y
graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution

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;
}

10
examples/OdometryExample.cpp
View File

@ -64,16 +64,16 @@ int main(int argc, char** argv) {
// 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 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, priorMean, priorNoise));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
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, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution

18
examples/PlanarSLAMExample.cpp
View File

@ -80,18 +80,18 @@ int main(int argc, char** argv) {
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.push_back(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.push_back(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),
@ -101,9 +101,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// Add Bearing-Range factors
graph.push_back(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.push_back(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.push_back(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
// Print
graph.print("Factor Graph:\n");

16
examples/Pose2SLAMExample.cpp
View File

@ -71,24 +71,24 @@ int main(int argc, char** argv) {
// 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)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, 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((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// 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:
graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(5, 2, 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

88
examples/Pose2SLAMExampleExpressions.cpp Normal file
View File

@ -0,0 +1,88 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMExampleExpressions.cpp
* @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/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <gtsam/nonlinear/NonlinearFactorGraph.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;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
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.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.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.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
// 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, -0.2 ));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
GaussNewtonParams parameters;
parameters.relativeErrorTol = 1e-5;
parameters.maxIterations = 100;
GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
// 5. Calculate and print marginal covariances for all variables
cout.precision(3);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl;
cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl;
return 0;
}

98
examples/Pose2SLAMExample_g2o.cpp Normal file
View File

@ -0,0 +1,98 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMExample_g2o.cpp
* @brief A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph and does the
* optimization. Output is written on a file, in g2o format
* Syntax for the script is ./Pose2SLAMExample_g2o input.g2o output.g2o
* @date May 15, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
using namespace std;
using namespace gtsam;
// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
int main(const int argc, const char *argv[]) {
string kernelType = "none";
int maxIterations = 100; // default
string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
// Parse user's inputs
if (argc > 1){
g2oFile = argv[1]; // input dataset filename
// outputFile = g2oFile = argv[2]; // done later
}
if (argc > 3){
maxIterations = atoi(argv[3]); // user can specify either tukey or huber
}
if (argc > 4){
kernelType = argv[4]; // user can specify either tukey or huber
}
// reading file and creating factor graph
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = false;
if(kernelType.compare("none") == 0){
boost::tie(graph, initial) = readG2o(g2oFile,is3D);
}
if(kernelType.compare("huber") == 0){
std::cout << "Using robust kernel: huber " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
}
if(kernelType.compare("tukey") == 0){
std::cout << "Using robust kernel: tukey " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
}
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Adding prior on pose 0 " << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION");
if (argc > 3) {
params.maxIterations = maxIterations;
std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
if (argc < 3) {
result.print("result");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

7
examples/Pose2SLAMExample_graph.cpp
View File

@ -31,13 +31,14 @@ int main (int argc, char** argv) {
// we are in build/examples, data is in examples/Data
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model);
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
string graph_file = findExampleDataFile("w100.graph");
boost::tie(graph, initial) = load2D(graph_file, model);
initial->print("Initial estimate:\n");
// Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01));
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
// Single Step Optimization using Levenberg-Marquardt

4
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -32,11 +32,11 @@ int main (int argc, char** argv) {
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, 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((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));

64
examples/Pose2SLAMExample_lago.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 Pose2SLAMExample_lago.cpp
* @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem
* using LAGO (Linear Approximation for Graph Optimization). See class lago.h
* Output is written on a file, in g2o format
* Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o
* @date May 15, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("noisyToyGraph.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.print();
std::cout << "Computing LAGO estimate" << std::endl;
Values estimateLago = lago::initialize(graphWithPrior);
std::cout << "done!" << std::endl;
if (argc < 3) {
estimateLago.print("estimateLago");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, estimateLago, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

8
examples/Pose2SLAMwSPCG.cpp
View File

@ -68,12 +68,12 @@ int main(int argc, char** argv) {
// 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((Vector(3) << 0.3, 0.3, 0.1));
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((Vector(3) << 0.2, 0.2, 0.1));
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));
@ -85,7 +85,7 @@ int main(int argc, char** argv) {
// 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((Vector(3) << 0.2, 0.2, 0.1));
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
@ -104,7 +104,7 @@ int main(int argc, char** argv) {
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
{
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();

89
examples/Pose3SLAMExample_changeKeys.cpp Normal file
View File

@ -0,0 +1,89 @@
/* ----------------------------------------------------------------------------
* 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 Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_changeKeys input.g2o rewritted.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
bool add = false;
Key firstKey = 8646911284551352320;
std::cout << "Using reference key: " << firstKey << std::endl;
if(add)
std::cout << "adding key " << std::endl;
else
std::cout << "subtracting key " << std::endl;
if (argc < 3) {
std::cout << "Please provide output file to write " << std::endl;
} else {
const string inputFileRewritten = argv[2];
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// Additional: rewrite input with simplified keys 0,1,...
Values simpleInitial;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
Key key;
if(add)
key = key_value.key + firstKey;
else
key = key_value.key - firstKey;
simpleInitial.insert(key, initial->at(key_value.key));
}
NonlinearFactorGraph simpleGraph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){
Key key1, key2;
if(add){
key1 = pose3Between->key1() + firstKey;
key2 = pose3Between->key2() + firstKey;
}else{
key1 = pose3Between->key1() - firstKey;
key2 = pose3Between->key2() - firstKey;
}
NonlinearFactor::shared_ptr simpleFactor(
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
simpleGraph.add(simpleFactor);
}
}
writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
}
return 0;
}

74
examples/Pose3SLAMExample_g2o.cpp Normal file
View File

@ -0,0 +1,74 @@
/* ----------------------------------------------------------------------------
* 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 Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
if (argc < 3) {
result.print("result");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, result, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

68
examples/Pose3SLAMExample_initializePose3Chordal.cpp Normal file
View File

@ -0,0 +1,68 @@
/* ----------------------------------------------------------------------------
* 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 Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
Values initialization = InitializePose3::initialize(graphWithPrior);
std::cout << "done!" << std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

72
examples/Pose3SLAMExample_initializePose3Gradient.cpp Normal file
View File

@ -0,0 +1,72 @@
/* ----------------------------------------------------------------------------
* 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 Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true;
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
std::cout << "done!" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

14
examples/RangeISAMExample_plaza2.cpp
View File

@ -40,6 +40,7 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
@ -59,9 +60,8 @@ namespace NM = gtsam::noiseModel;
typedef pair<double, Pose2> TimedOdometry;
list<TimedOdometry> readOdometry() {
list<TimedOdometry> odometryList;
ifstream is("../../examples/Data/Plaza2_DR.txt");
if (!is)
throw runtime_error("../../examples/Data/Plaza2_DR.txt file not found");
string data_file = findExampleDataFile("Plaza2_DR.txt");
ifstream is(data_file.c_str());
while (is) {
double t, distance_traveled, delta_heading;
@ -78,12 +78,12 @@ list<TimedOdometry> readOdometry() {
typedef boost::tuple<double, size_t, double> RangeTriple;
vector<RangeTriple> readTriples() {
vector<RangeTriple> triples;
ifstream is("../../examples/Data/Plaza2_TD.txt");
if (!is)
throw runtime_error("../../examples/Data/Plaza2_TD.txt file not found");
string data_file = findExampleDataFile("Plaza2_TD.txt");
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));
}

6
examples/SFMExample.cpp
View File

@ -80,13 +80,13 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 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()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
@ -111,6 +111,8 @@ int main(int argc, char* argv[]) {
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
result.print("Final results:\n");
cout << "initial error = " << graph.error(initialEstimate) << endl;
cout << "final error = " << graph.error(result) << endl;
return 0;
}

101
examples/SFMExampleExpressions.cpp Normal file
View File

@ -0,0 +1,101 @@
/* ----------------------------------------------------------------------------
* 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.cpp
* @brief A structure-from-motion example done with Expressions
* @author Frank Dellaert
* @author Duy-Nguyen Ta
* @date October 1, 2014
*/
/**
* 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 <examples/SFMdata.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <vector>
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);
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
ExpressionFactorGraph graph;
// Specify uncertainty on first pose prior
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
// x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
Pose3_ x0('x',0);
graph.addExpressionFactor(x0, poses[0], poseNoise);
// We create a constant Expression for the calibration here
Cal3_S2_ cK(K);
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
Pose3_ x('x', i);
SimpleCamera camera(poses[i], K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
// 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 an ExpressionFactor
graph.addExpressionFactor(prediction, measurement, measurementNoise);
}
}
// Add prior on first point to constrain scale, again with ExpressionFactor
Isotropic::shared_ptr pointNoise = Isotropic::Sigma(3, 0.1);
graph.addExpressionFactor(Point3_('l', 0), points[0], pointNoise);
// Create perturbed initial
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)
initial.insert(Symbol('x', i), poses[i].compose(delta));
for (size_t j = 0; j < points.size(); ++j)
initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
cout << "initial error = " << graph.error(initial) << endl;
/* Optimize the graph and print results */
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;
}
/* ************************************************************************* */

157
examples/SFMExample_SmartFactor.cpp Normal file
View File

@ -0,0 +1,157 @@
/* ----------------------------------------------------------------------------
* 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_SmartFactor.cpp
* @brief A structure-from-motion problem on a simulated dataset, using smart projection factor
* @author Duy-Nguyen Ta
* @author Jing Dong
* @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.
#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>
using namespace std;
using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::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:
// 1. the 2D measurement
// 2. the corresponding camera's key
// 3. camera noise model
// 4. camera calibration
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));
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise)); // add directly to graph
graph.print("Factor Graph:\n");
// Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
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));
initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
result.print("Final results:\n");
// A smart factor represent the 3D point inside the factor, not as a variable.
// The 3D position of the landmark is not explicitly calculated by the optimizer.
// To obtain the landmark's 3D position, we use the "point" method of the smart factor.
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);
if (point) // ignore if boost::optional return NULL
landmark_result.insert(j, *point);
}
}
landmark_result.print("Landmark results:\n");
return 0;
}
/* ************************************************************************* */

3
examples/SFMExample_bal.cpp
View File

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

4
examples/SelfCalibrationExample.cpp
View File

@ -53,7 +53,7 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 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()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Simulated measurements from each camera pose, adding them to the factor graph
@ -74,7 +74,7 @@ int main(int argc, char* argv[]) {
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100));
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
// Create the initial estimate to the solution

63
examples/SolverComparer.cpp
View File

@ -13,6 +13,22 @@
* @brief Incremental and batch solving, timing, and accuracy comparisons
* @author Richard Roberts
* @date August, 2013
*
* Here is an example. Below, to run in batch mode, we first generate an initialization in incremental mode.
*
* Solve in incremental and write to file w_inc:
* ./SolverComparer --incremental -d w10000 -o w_inc
*
* You can then perturb that initialization to get batch something to optimize.
* Read in w_inc, perturb it with noise of stddev 0.6, and write to w_pert:
* ./SolverComparer --perturb 0.6 -i w_inc -o w_pert
*
* Then optimize with batch, read in w_pert, solve in batch, and write to w_batch:
* ./SolverComparer --batch -d w10000 -i w_pert -o w_batch
*
* And finally compare solutions in w_inc and w_batch to check that batch converged to the global minimum
* ./SolverComparer --compare w_inc w_batch
*
*/
#include <gtsam/base/timing.h>
@ -56,22 +72,22 @@ 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);
//GTSAM_VALUE_EXPORT(Value);
//GTSAM_VALUE_EXPORT(Pose);
//GTSAM_VALUE_EXPORT(Rot2);
//GTSAM_VALUE_EXPORT(Point2);
//GTSAM_VALUE_EXPORT(NonlinearFactor);
//GTSAM_VALUE_EXPORT(NoiseModelFactor);
//GTSAM_VALUE_EXPORT(NM1);
//GTSAM_VALUE_EXPORT(NM2);
//GTSAM_VALUE_EXPORT(BetweenFactor<Pose>);
//GTSAM_VALUE_EXPORT(PriorFactor<Pose>);
//GTSAM_VALUE_EXPORT(BR);
//GTSAM_VALUE_EXPORT(noiseModel::Base);
//GTSAM_VALUE_EXPORT(noiseModel::Isotropic);
//GTSAM_VALUE_EXPORT(noiseModel::Gaussian);
//GTSAM_VALUE_EXPORT(noiseModel::Diagonal);
//GTSAM_VALUE_EXPORT(noiseModel::Unit);
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
// Compute degrees of freedom (observations - variables)
@ -95,6 +111,7 @@ int lastStep;
int nThreads;
int relinSkip;
bool incremental;
bool dogleg;
bool batch;
bool compare;
bool perturb;
@ -126,6 +143,7 @@ int main(int argc, char *argv[]) {
("threads", po::value<int>(&nThreads)->default_value(-1), "Number of threads, or -1 to use all processors")
("relinSkip", po::value<int>(&relinSkip)->default_value(10), "Fluid relinearization check every arg steps")
("incremental", "Run in incremental mode using ISAM2 (default)")
("dogleg", "When in incremental mode, solve with Dogleg instead of Gauss-Newton in iSAM2")
("batch", "Run in batch mode, requires an initialization from --read-solution")
("compare", po::value<vector<string> >()->multitoken(), "Compare two solution files")
("perturb", po::value<double>(&perturbationNoise), "Perturb a solution file with the specified noise")
@ -141,6 +159,7 @@ int main(int argc, char *argv[]) {
stats = (vm.count("stats") > 0);
const int modesSpecified = int(batch) + int(compare) + int(perturb) + int(stats);
incremental = (vm.count("incremental") > 0 || modesSpecified == 0);
dogleg = (vm.count("dogleg") > 0);
if(compare) {
const vector<string>& compareFiles = vm["compare"].as<vector<string> >();
if(compareFiles.size() != 2) {
@ -233,6 +252,8 @@ int main(int argc, char *argv[]) {
void runIncremental()
{
ISAM2Params params;
if(dogleg)
params.optimizationParams = ISAM2DoglegParams();
params.relinearizeSkip = relinSkip;
params.enablePartialRelinearizationCheck = true;
ISAM2 isam2(params);
@ -274,7 +295,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);
@ -453,7 +474,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;
@ -536,8 +557,8 @@ void runCompare()
void runPerturb()
{
// Set up random number generator
boost::random::mt19937 rng;
boost::random::normal_distribution<double> normal(0.0, perturbationNoise);
boost::mt19937 rng;
boost::normal_distribution<double> normal(0.0, perturbationNoise);
// Perturb values
VectorValues noise;
@ -568,7 +589,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;

76
examples/StereoVOExample.cpp Normal file
View File

@ -0,0 +1,76 @@
/* ----------------------------------------------------------------------------
* 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 SteroVOExample.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
*/
/**
* A 3D stereo visual odometry example
* - robot starts at origin
* -moves forward 1 meter
* -takes stereo readings on three landmarks
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/StereoFactor.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose;
graph.push_back(NonlinearEquality<Pose3>(1, Pose3()));
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
//create stereo camera calibration object with .2m between cameras
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
//create and add stereo factors between second pose and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
//create Values object to contain initial estimates of camera poses and landmark locations
Values initial_estimate;
//create and add iniital estimates
initial_estimate.insert(1, first_pose);
initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
initial_estimate.insert(3, Point3(1, 1, 5));
initial_estimate.insert(4, Point3(-1, 1, 5));
initial_estimate.insert(5, Point3(0, -0.5, 5));
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
Values result = optimizer.optimize();
result.print("Final result:\n");
return 0;
}

116
examples/StereoVOExample_large.cpp Normal file
View File

@ -0,0 +1,116 @@
/* ----------------------------------------------------------------------------
* 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 SteroVOExample.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
*/
/**
* A 3D stereo visual odometry example
* - robot starts at origin
* -moves forward, taking periodic stereo measurements
* -takes stereo readings of many landmarks
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/dataset.h>
#include <string>
#include <fstream>
#include <iostream>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
//read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
double fx, fy, s, u0, v0, b;
ifstream calibration_file(calibration_loc.c_str());
cout << "Reading calibration info" << endl;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
//create stereo camera calibration object
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
ifstream pose_file(pose_loc.c_str());
cout << "Reading camera poses" << endl;
int pose_id;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> pose_id) {
for (int i = 0; i < 16; i++) {
pose_file >> m.data()[i];
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
}
// camera and landmark keys
size_t x, l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.push_back(
GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint);
}
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtParams params;
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n");
return 0;
}

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;
}

2
examples/VisualISAM2Example.cpp
View File

@ -103,7 +103,7 @@ int main(int argc, char* argv[]) {
// adding it to iSAM.
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 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()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0

39
examples/VisualISAMExample.cpp
View File

@ -14,6 +14,7 @@
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
* This version uses iSAM to solve the problem incrementally
* @author Duy-Nguyen Ta
* @author Frank Dellaert
*/
/**
@ -61,7 +62,8 @@ int main(int argc, char* argv[]) {
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
noiseModel::Isotropic::shared_ptr noise = //
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
@ -69,7 +71,8 @@ int main(int argc, char* argv[]) {
// Create the set of ground-truth poses
vector<Pose3> poses = createPoses();
// Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates
// Create a NonlinearISAM object which will relinearize and reorder the variables
// every "relinearizeInterval" updates
int relinearizeInterval = 3;
NonlinearISAM isam(relinearizeInterval);
@ -82,32 +85,44 @@ int main(int argc, char* argv[]) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
// Add measurement
graph.add(
GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
Symbol('x', i), Symbol('l', j), K));
}
// Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 initial_xi = poses[i].compose(noise);
// Add an initial guess for the current pose
initialEstimate.insert(Symbol('x', i), initial_xi);
// If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale
// Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
// adding it to iSAM.
if (i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on pose x0, with 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.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
noiseModel::Isotropic::shared_ptr pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
// Add initial guesses to all observed landmarks
Point3 noise(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j) {
// Intentionally initialize the variables off from the ground truth
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
Point3 initial_lj = points[j].compose(noise);
initialEstimate.insert(Symbol('l', j), initial_lj);
}
} else {
// Update iSAM with the new factors

8
examples/easyPoint2KalmanFilter.cpp
View File

@ -37,7 +37,7 @@ int main() {
// Create the Kalman Filter initialization point
Point2 x_initial(0.0, 0.0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1));
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
// Create Key for initial pose
Symbol x0('x',0);
@ -57,8 +57,8 @@ int main() {
// For the purposes of this example, let us assume we are using a constant-position model and
// the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1]
// and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1].
Vector u = (Vector(2) << 1.0, 0.0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true);
Vector u = Vector2(1.0, 0.0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true);
// This simple motion can be modeled with a BetweenFactor
// Create Key for next pose
@ -83,7 +83,7 @@ int main() {
// For the purposes of this example, let us assume we have something like a GPS that returns
// the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise
// R = [0.25 0 ; 0 0.25].
SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25), true);
SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true);
// This simple measurement can be modeled with a PriorFactor
Point2 z1(1.0, 0.0);

438
gtsam.h
View File

@ -157,7 +157,7 @@ virtual class Value {
};
#include <gtsam/base/LieScalar.h>
virtual class LieScalar : gtsam::Value {
class LieScalar {
// Standard constructors
LieScalar();
LieScalar(double d);
@ -186,7 +186,7 @@ virtual class LieScalar : gtsam::Value {
};
#include <gtsam/base/LieVector.h>
virtual class LieVector : gtsam::Value {
class LieVector {
// Standard constructors
LieVector();
LieVector(Vector v);
@ -218,7 +218,7 @@ virtual class LieVector : gtsam::Value {
};
#include <gtsam/base/LieMatrix.h>
virtual class LieMatrix : gtsam::Value {
class LieMatrix {
// Standard constructors
LieMatrix();
LieMatrix(Matrix v);
@ -253,7 +253,7 @@ virtual class LieMatrix : gtsam::Value {
// geometry
//*************************************************************************
virtual class Point2 : gtsam::Value {
class Point2 {
// Standard Constructors
Point2();
Point2(double x, double y);
@ -270,8 +270,6 @@ virtual class Point2 : gtsam::Value {
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;
@ -290,7 +288,7 @@ virtual class Point2 : gtsam::Value {
void serialize() const;
};
virtual class StereoPoint2 : gtsam::Value {
class StereoPoint2 {
// Standard Constructors
StereoPoint2();
StereoPoint2(double uL, double uR, double v);
@ -325,7 +323,7 @@ virtual class StereoPoint2 : gtsam::Value {
void serialize() const;
};
virtual class Point3 : gtsam::Value {
class Point3 {
// Standard Constructors
Point3();
Point3(double x, double y, double z);
@ -342,8 +340,6 @@ virtual class Point3 : gtsam::Value {
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;
@ -361,7 +357,7 @@ virtual class Point3 : gtsam::Value {
void serialize() const;
};
virtual class Rot2 : gtsam::Value {
class Rot2 {
// Standard Constructors and Named Constructors
Rot2();
Rot2(double theta);
@ -380,8 +376,6 @@ virtual class Rot2 : gtsam::Value {
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;
@ -406,7 +400,7 @@ virtual class Rot2 : gtsam::Value {
void serialize() const;
};
virtual class Rot3 : gtsam::Value {
class Rot3 {
// Standard Constructors and Named Constructors
Rot3();
Rot3(Matrix R);
@ -433,8 +427,6 @@ virtual class Rot3 : gtsam::Value {
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;
@ -462,7 +454,7 @@ virtual class Rot3 : gtsam::Value {
void serialize() const;
};
virtual class Pose2 : gtsam::Value {
class Pose2 {
// Standard Constructor
Pose2();
Pose2(const gtsam::Pose2& pose);
@ -482,8 +474,6 @@ virtual class Pose2 : gtsam::Value {
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;
@ -512,7 +502,7 @@ virtual class Pose2 : gtsam::Value {
void serialize() const;
};
virtual class Pose3 : gtsam::Value {
class Pose3 {
// Standard Constructors
Pose3();
Pose3(const gtsam::Pose3& pose);
@ -531,10 +521,7 @@ virtual class Pose3 : gtsam::Value {
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
@ -563,15 +550,15 @@ virtual class Pose3 : gtsam::Value {
void serialize() const;
};
#include <gtsam/geometry/Sphere2.h>
virtual class Sphere2 : gtsam::Value {
#include <gtsam/geometry/Unit3.h>
class Unit3 {
// Standard Constructors
Sphere2();
Sphere2(const gtsam::Point3& pose);
Unit3();
Unit3(const gtsam::Point3& pose);
// Testable
void print(string s) const;
bool equals(const gtsam::Sphere2& pose, double tol) const;
bool equals(const gtsam::Unit3& pose, double tol) const;
// Other functionality
Matrix basis() const;
@ -580,14 +567,14 @@ virtual class Sphere2 : gtsam::Value {
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Sphere2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Sphere2& s) const;
gtsam::Unit3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Unit3& s) const;
};
#include <gtsam/geometry/EssentialMatrix.h>
virtual class EssentialMatrix : gtsam::Value {
class EssentialMatrix {
// Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Sphere2& aTb);
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
// Testable
void print(string s) const;
@ -601,12 +588,12 @@ virtual class EssentialMatrix : gtsam::Value {
// Other methods:
gtsam::Rot3 rotation() const;
gtsam::Sphere2 direction() const;
gtsam::Unit3 direction() const;
Matrix matrix() const;
double error(Vector vA, Vector vB);
};
virtual class Cal3_S2 : gtsam::Value {
class Cal3_S2 {
// Standard Constructors
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
@ -642,28 +629,13 @@ virtual class Cal3_S2 : gtsam::Value {
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Value {
#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;
@ -671,14 +643,66 @@ virtual class Cal3DS2 : gtsam::Value {
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();
@ -699,7 +723,43 @@ class Cal3_S2Stereo {
double baseline() const;
};
virtual class CalibratedCamera : gtsam::Value {
#include <gtsam/geometry/Cal3Bundler.h>
class Cal3Bundler {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3Bundler retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Bundler& 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;
// Standard Interface
double fx() const;
double fy() const;
double k1() const;
double k2() const;
double u0() const;
double v0() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
// enabling serialization functionality
void serialize() const;
};
class CalibratedCamera {
// Standard Constructors and Named Constructors
CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose);
@ -716,10 +776,6 @@ virtual class CalibratedCamera : gtsam::Value {
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);
@ -732,7 +788,7 @@ virtual class CalibratedCamera : gtsam::Value {
void serialize() const;
};
virtual class SimpleCamera : gtsam::Value {
class SimpleCamera {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
@ -771,7 +827,7 @@ virtual class SimpleCamera : gtsam::Value {
};
template<CALIBRATION = {gtsam::Cal3DS2}>
virtual class PinholeCamera : gtsam::Value {
class PinholeCamera {
// Standard Constructors and Named Constructors
PinholeCamera();
PinholeCamera(const gtsam::Pose3& pose);
@ -809,7 +865,7 @@ virtual class PinholeCamera : gtsam::Value {
void serialize() const;
};
virtual class StereoCamera : gtsam::Value {
class StereoCamera {
// Standard Constructors and Named Constructors
StereoCamera();
StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
@ -862,7 +918,7 @@ virtual class SymbolicFactor {
};
#include <gtsam/symbolic/SymbolicFactorGraph.h>
class SymbolicFactorGraph {
virtual class SymbolicFactorGraph {
SymbolicFactorGraph();
SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
@ -1192,6 +1248,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;
@ -1423,6 +1480,7 @@ virtual class GaussianBayesNet {
void push_back(const gtsam::GaussianBayesNet& bayesNet);
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
@ -1480,9 +1538,7 @@ class GaussianISAM {
#include <gtsam/linear/IterativeSolver.h>
virtual class IterativeOptimizationParameters {
string getKernel() const ;
string getVerbosity() const;
void setKernel(string s) ;
void setVerbosity(string s) ;
void print() const;
};
@ -1515,7 +1571,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
void print() const;
};
class SubgraphSolver {
virtual class SubgraphSolver {
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
gtsam::VectorValues optimize() const;
@ -1550,6 +1606,10 @@ char symbolChr(size_t key);
size_t symbolIndex(size_t key);
// Default keyformatter
void printKeyList (const gtsam::KeyList& keys);
void printKeyList (const gtsam::KeyList& keys, string s);
void printKeyVector(const gtsam::KeyVector& keys);
void printKeyVector(const gtsam::KeyVector& keys, string s);
void printKeySet (const gtsam::KeySet& keys);
void printKeySet (const gtsam::KeySet& keys, string s);
@ -1612,6 +1672,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;
@ -1661,15 +1722,12 @@ class Values {
void print(string s) const;
bool equals(const gtsam::Values& other, double tol) const;
void insert(size_t j, const gtsam::Value& value);
void insert(const gtsam::Values& values);
void update(size_t j, const gtsam::Value& val);
void update(const gtsam::Values& values);
void erase(size_t j);
void swap(gtsam::Values& values);
bool exists(size_t j) const;
gtsam::Value at(size_t j) const;
gtsam::KeyList keys() const;
gtsam::VectorValues zeroVectors() const;
@ -1679,6 +1737,57 @@ class Values {
// enabling serialization functionality
void serialize() const;
// New in 4.0, we have to specialize every insert/update/at to generate wrappers
// Instead of the old:
// 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);
void insert(size_t j, const gtsam::Pose2& t);
void insert(size_t j, const gtsam::Rot3& t);
void insert(size_t j, const gtsam::Pose3& t);
void insert(size_t j, const gtsam::Cal3_S2& t);
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);
void update(size_t j, const gtsam::Rot2& t);
void update(size_t j, const gtsam::Pose2& t);
void update(size_t j, const gtsam::Rot3& t);
void update(size_t j, const gtsam::Pose3& t);
void update(size_t j, const gtsam::Cal3_S2& t);
void update(size_t j, const gtsam::Cal3DS2& t);
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::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>
@ -1725,6 +1834,7 @@ class KeySet {
// structure specific methods
void insert(size_t key);
void merge(gtsam::KeySet& other);
bool erase(size_t key); // returns true if value was removed
bool count(size_t key) const; // returns true if value exists
@ -1775,6 +1885,7 @@ class KeyGroupMap {
class Marginals {
Marginals(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& solution);
void print(string s) const;
Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const;
@ -1849,12 +1960,12 @@ virtual class NonlinearOptimizerParams {
void setLinearSolverType(string solver);
void setOrdering(const gtsam::Ordering& ordering);
void setIterativeParams(const gtsam::SubgraphSolverParameters &params);
void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
bool isMultifrontal() const;
bool isSequential() const;
bool isCholmod() const;
bool isCG() const;
bool isIterative() const;
};
bool checkConvergence(double relativeErrorTreshold,
@ -2073,7 +2184,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, 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;
@ -2084,7 +2195,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const;
@ -2094,8 +2205,9 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, 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 = {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 NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible);
@ -2117,10 +2229,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>
@ -2140,6 +2256,8 @@ template<POSE, POINT, ROTATION>
virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
BearingRangeFactor(size_t poseKey, size_t pointKey, const ROTATION& measuredBearing, double measuredRange, const gtsam::noiseModel::Base* noiseModel);
pair<ROTATION, double> measured() const;
// enabling serialization functionality
void serialize() const;
};
@ -2191,6 +2309,25 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
void serialize() const;
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template<POSE, CALIBRATION>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
SmartProjectionPoseFactor(double rankTol, double linThreshold,
bool manageDegeneracy, bool enableEPI, const POSE& 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);
// enabling serialization functionality
//void serialize() const;
};
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
#include <gtsam/slam/StereoFactor.h>
template<POSE, LANDMARK>
@ -2241,6 +2378,13 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
gtsam::noiseModel::Base* model);
void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
//*************************************************************************
// Navigation
@ -2248,7 +2392,7 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename
namespace imuBias {
#include <gtsam/navigation/ImuBias.h>
virtual class ConstantBias : gtsam::Value {
class ConstantBias {
// Standard Constructor
ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro);
@ -2264,8 +2408,6 @@ virtual class ConstantBias : gtsam::Value {
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;
@ -2284,18 +2426,36 @@ virtual class ConstantBias : gtsam::Value {
}///\namespace imuBias
#include <gtsam/navigation/ImuFactor.h>
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);
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 ImuFactor : gtsam::NonlinearFactor {
@ -2304,13 +2464,8 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam/navigation/CombinedImuFactor.h>
@ -2324,28 +2479,110 @@ class CombinedImuFactorPreintegratedMeasurements {
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
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,
const gtsam::noiseModel::Base* model);
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam/navigation/AHRSFactor.h>
class AHRSFactorPreintegratedMeasurements {
// Standard Constructor
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
// get Data
Matrix deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
// Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
void resetIntegration() ;
};
virtual class AHRSFactor : gtsam::NonlinearFactor {
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const;
};
#include <gtsam/navigation/AttitudeFactor.h>
//virtual class AttitudeFactor : gtsam::NonlinearFactor {
// AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
// AttitudeFactor();
//};
virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Rot3AttitudeFactor();
void print(string s) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Pose3AttitudeFactor();
void print(string s) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
//*************************************************************************
@ -2355,17 +2592,26 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
namespace utilities {
#include <matlab.h>
gtsam::KeyList createKeyList(Vector I);
gtsam::KeyList createKeyList(string s, Vector I);
gtsam::KeyVector createKeyVector(Vector I);
gtsam::KeyVector createKeyVector(string s, Vector I);
gtsam::KeySet createKeySet(Vector I);
gtsam::KeySet createKeySet(string s, Vector I);
Matrix extractPoint2(const gtsam::Values& values);
Matrix extractPoint3(const gtsam::Values& values);
Matrix extractPose2(const gtsam::Values& values);
gtsam::Values allPose3s(gtsam::Values& values);
Matrix extractPose3(const gtsam::Values& values);
void perturbPoint2(gtsam::Values& values, double sigma, int seed);
void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed);
void perturbPoint3(gtsam::Values& values, double sigma, int seed);
void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
} //\namespace utilities

BIN
gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o vendored
View File

Binary file not shown.

BIN
gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o vendored
View File

Binary file not shown.

BIN
gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o vendored
View File

Binary file not shown.

3
gtsam/3rdparty/CCOLAMD/Source/ccolamd.c vendored
View File

@ -617,11 +617,12 @@
#include "ccolamd.h"
#include <stdlib.h>
#include <math.h>
#include <limits.h>
#ifdef MATLAB_MEX_FILE
#include <stdint.h>
typedef uint16_t char16_t;
#include "mex.h"
#include "matrix.h"
#endif

3
gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c vendored
View File

@ -13,6 +13,9 @@
#ifndef NPRINT
#ifdef MATLAB_MEX_FILE
#include <stdlib.h>
#include <stdint.h>
typedef uint16_t char16_t;
#include "mex.h"
int (*ccolamd_printf) (const char *, ...) = mexPrintf ;
#else

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

@ -16,11 +16,62 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
endif()
endforeach(eigen_dir)
# 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)
if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn")))
list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}")
install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen)
endif()
endforeach(unsupported_eigen_dir)
# Add to project source
set(eigen_headers ${eigen_headers} PARENT_SCOPE)
# set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE)
# install Eigen - only the headers in our 3rdparty directory
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_EXECUTABLES "Build metis library executables" OFF)
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)
# set (TOOLS CartConvert ConicProj GeodesicProj GeoConvert GeodSolve
# GeoidEval Gravity MagneticField Planimeter TransverseMercatorProj)
# add_subdirectory (tools)
# Find GeographicLib using the find script distributed with it
include(GeographicLib/cmake/FindGeographicLib.cmake)
# Set up the option to install GeographicLib
if(GEOGRAPHICLIB_FOUND)
set(install_geographiclib_default OFF)
else()
set(install_geographiclib_default ON)
endif()
option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default})
# Print warning if we'll overwrite GeographicLib
if(GEOGRAPHICLIB_FOUND AND GTSAM_INSTALL_GEOGRAPHICLIB)
message(WARNING "GeographicLib is installed on your system and GTSAM_INSTALL_GEOGRAPHICLIB is enabled. Installing gtsam will either overwrite the installed GeographicLib or install a second version that may conflict. You may want to disable GTSAM_INSTALL_GEOGRAPHICLIB if the installed version was not installed by GTSAM.")
endif()
if(GTSAM_INSTALL_GEOGRAPHICLIB)
add_subdirectory(GeographicLib)
endif()
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

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

@ -204,7 +204,7 @@ if(NOT MSVC)
option(EIGEN_TEST_NEON "Enable/Disable Neon in tests/examples" OFF)
if(EIGEN_TEST_NEON)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a"8)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -mcpu=cortex-a8")
message(STATUS "Enabling NEON in tests/examples")
endif()

8
gtsam/3rdparty/Eigen/CTestConfig.cmake vendored
View File

@ -4,14 +4,10 @@
## # The following are required to uses Dart and the Cdash dashboard
## ENABLE_TESTING()
## INCLUDE(CTest)
set(CTEST_PROJECT_NAME "Eigen")
set(CTEST_PROJECT_NAME "Eigen3.2")
set(CTEST_NIGHTLY_START_TIME "00:00:00 UTC")
set(CTEST_DROP_METHOD "http")
set(CTEST_DROP_SITE "manao.inria.fr")
set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen")
set(CTEST_DROP_LOCATION "/CDash/submit.php?project=Eigen3.2")
set(CTEST_DROP_SITE_CDASH TRUE)
set(CTEST_PROJECT_SUBPROJECTS
Official
Unsupported
)

4
gtsam/3rdparty/Eigen/Eigen/Core vendored
View File

@ -95,7 +95,7 @@
extern "C" {
// In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
// Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
#ifdef __INTEL_COMPILER
#if defined(__INTEL_COMPILER) && __INTEL_COMPILER >= 1110
#include <immintrin.h>
#else
#include <emmintrin.h>
@ -165,7 +165,7 @@
#endif
// required for __cpuid, needs to be included after cmath
#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64))
#if defined(_MSC_VER) && (defined(_M_IX86)||defined(_M_X64)) && (!defined(_WIN32_WCE))
#include <intrin.h>
#endif

13
gtsam/3rdparty/Eigen/Eigen/Eigen2Support vendored
View File

@ -14,10 +14,23 @@
#error Eigen2 support must be enabled by defining EIGEN2_SUPPORT before including any Eigen header
#endif
#ifndef EIGEN_NO_EIGEN2_DEPRECATED_WARNING
#if defined(__GNUC__) || defined(__INTEL_COMPILER) || defined(__clang__)
#warning "Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)"
#else
#pragma message ("Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3. (Define EIGEN_NO_EIGEN2_DEPRECATED_WARNING to disable this warning)")
#endif
#endif // EIGEN_NO_EIGEN2_DEPRECATED_WARNING
#include "src/Core/util/DisableStupidWarnings.h"
/** \ingroup Support_modules
* \defgroup Eigen2Support_Module Eigen2 support module
*
* \warning Eigen2 support is deprecated in Eigen 3.2.x and it will be removed in Eigen 3.3.
*
* This module provides a couple of deprecated functions improving the compatibility with Eigen2.
*
* To use it, define EIGEN2_SUPPORT before including any Eigen header

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

@ -17,6 +17,9 @@ namespace Eigen {
namespace internal {
template<typename MatrixType, int UpLo> struct LDLT_Traits;
// PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef
enum SignMatrix { PositiveSemiDef, NegativeSemiDef, ZeroSign, Indefinite };
}
/** \ingroup Cholesky_Module
@ -69,7 +72,12 @@ template<typename _MatrixType, int _UpLo> class LDLT
* The default constructor is useful in cases in which the user intends to
* perform decompositions via LDLT::compute(const MatrixType&).
*/
LDLT() : m_matrix(), m_transpositions(), m_isInitialized(false) {}
LDLT()
: m_matrix(),
m_transpositions(),
m_sign(internal::ZeroSign),
m_isInitialized(false)
{}
/** \brief Default Constructor with memory preallocation
*
@ -81,6 +89,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
: m_matrix(size, size),
m_transpositions(size),
m_temporary(size),
m_sign(internal::ZeroSign),
m_isInitialized(false)
{}
@ -93,6 +102,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
: m_matrix(matrix.rows(), matrix.cols()),
m_transpositions(matrix.rows()),
m_temporary(matrix.rows()),
m_sign(internal::ZeroSign),
m_isInitialized(false)
{
compute(matrix);
@ -139,7 +149,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
inline bool isPositive() const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return m_sign == 1;
return m_sign == internal::PositiveSemiDef || m_sign == internal::ZeroSign;
}
#ifdef EIGEN2_SUPPORT
@ -153,7 +163,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
inline bool isNegative(void) const
{
eigen_assert(m_isInitialized && "LDLT is not initialized.");
return m_sign == -1;
return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign;
}
/** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
@ -235,7 +245,7 @@ template<typename _MatrixType, int _UpLo> class LDLT
MatrixType m_matrix;
TranspositionType m_transpositions;
TmpMatrixType m_temporary;
int m_sign;
internal::SignMatrix m_sign;
bool m_isInitialized;
};
@ -246,7 +256,7 @@ template<int UpLo> struct ldlt_inplace;
template<> struct ldlt_inplace<Lower>
{
template<typename MatrixType, typename TranspositionType, typename Workspace>
static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
static bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
{
using std::abs;
typedef typename MatrixType::Scalar Scalar;
@ -258,36 +268,19 @@ template<> struct ldlt_inplace<Lower>
if (size <= 1)
{
transpositions.setIdentity();
if(sign)
*sign = numext::real(mat.coeff(0,0))>0 ? 1:-1;
if (numext::real(mat.coeff(0,0)) > 0) sign = PositiveSemiDef;
else if (numext::real(mat.coeff(0,0)) < 0) sign = NegativeSemiDef;
else sign = ZeroSign;
return true;
}
RealScalar cutoff(0), biggest_in_corner;
for (Index k = 0; k < size; ++k)
{
// Find largest diagonal element
Index index_of_biggest_in_corner;
biggest_in_corner = mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
mat.diagonal().tail(size-k).cwiseAbs().maxCoeff(&index_of_biggest_in_corner);
index_of_biggest_in_corner += k;
if(k == 0)
{
// The biggest overall is the point of reference to which further diagonals
// are compared; if any diagonal is negligible compared
// to the largest overall, the algorithm bails.
cutoff = abs(NumTraits<Scalar>::epsilon() * biggest_in_corner);
}
// Finish early if the matrix is not full rank.
if(biggest_in_corner < cutoff)
{
for(Index i = k; i < size; i++) transpositions.coeffRef(i) = i;
if(sign) *sign = 0;
break;
}
transpositions.coeffRef(k) = index_of_biggest_in_corner;
if(k != index_of_biggest_in_corner)
{
@ -318,22 +311,27 @@ template<> struct ldlt_inplace<Lower>
if(k>0)
{
temp.head(k) = mat.diagonal().head(k).asDiagonal() * A10.adjoint();
temp.head(k) = mat.diagonal().real().head(k).asDiagonal() * A10.adjoint();
mat.coeffRef(k,k) -= (A10 * temp.head(k)).value();
if(rs>0)
A21.noalias() -= A20 * temp.head(k);
}
if((rs>0) && (abs(mat.coeffRef(k,k)) > cutoff))
A21 /= mat.coeffRef(k,k);
if(sign)
{
// LDLT is not guaranteed to work for indefinite matrices, but let's try to get the sign right
int newSign = numext::real(mat.diagonal().coeff(index_of_biggest_in_corner)) > 0;
if(k == 0)
*sign = newSign;
else if(*sign != newSign)
*sign = 0;
// In some previous versions of Eigen (e.g., 3.2.1), the scaling was omitted if the pivot
// was smaller than the cutoff value. However, soince LDLT is not rank-revealing
// we should only make sure we do not introduce INF or NaN values.
// LAPACK also uses 0 as the cutoff value.
RealScalar realAkk = numext::real(mat.coeffRef(k,k));
if((rs>0) && (abs(realAkk) > RealScalar(0)))
A21 /= realAkk;
if (sign == PositiveSemiDef) {
if (realAkk < 0) sign = Indefinite;
} else if (sign == NegativeSemiDef) {
if (realAkk > 0) sign = Indefinite;
} else if (sign == ZeroSign) {
if (realAkk > 0) sign = PositiveSemiDef;
else if (realAkk < 0) sign = NegativeSemiDef;
}
}
@ -399,7 +397,7 @@ template<> struct ldlt_inplace<Lower>
template<> struct ldlt_inplace<Upper>
{
template<typename MatrixType, typename TranspositionType, typename Workspace>
static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, int* sign=0)
static EIGEN_STRONG_INLINE bool unblocked(MatrixType& mat, TranspositionType& transpositions, Workspace& temp, SignMatrix& sign)
{
Transpose<MatrixType> matt(mat);
return ldlt_inplace<Lower>::unblocked(matt, transpositions, temp, sign);
@ -445,7 +443,7 @@ LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::compute(const MatrixType& a)
m_isInitialized = false;
m_temporary.resize(size);
internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, &m_sign);
internal::ldlt_inplace<UpLo>::unblocked(m_matrix, m_transpositions, m_temporary, m_sign);
m_isInitialized = true;
return *this;
@ -473,7 +471,7 @@ LDLT<MatrixType,_UpLo>& LDLT<MatrixType,_UpLo>::rankUpdate(const MatrixBase<Deri
for (Index i = 0; i < size; i++)
m_transpositions.coeffRef(i) = i;
m_temporary.resize(size);
m_sign = sigma>=0 ? 1 : -1;
m_sign = sigma>=0 ? internal::PositiveSemiDef : internal::NegativeSemiDef;
m_isInitialized = true;
}
@ -506,9 +504,15 @@ struct solve_retval<LDLT<_MatrixType,_UpLo>, Rhs>
typedef typename LDLTType::MatrixType MatrixType;
typedef typename LDLTType::Scalar Scalar;
typedef typename LDLTType::RealScalar RealScalar;
const Diagonal<const MatrixType> vectorD = dec().vectorD();
RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() * NumTraits<Scalar>::epsilon(),
RealScalar(1) / NumTraits<RealScalar>::highest()); // motivated by LAPACK's xGELSS
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
// as motivated by LAPACK's xGELSS:
// RealScalar tolerance = (max)(vectorD.array().abs().maxCoeff() *NumTraits<RealScalar>::epsilon(),RealScalar(1) / NumTraits<RealScalar>::highest());
// However, LDLT is not rank revealing, and so adjusting the tolerance wrt to the highest
// diagonal element is not well justified and to numerical issues in some cases.
// Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
RealScalar tolerance = RealScalar(1) / NumTraits<RealScalar>::highest();
for (Index i = 0; i < vectorD.size(); ++i) {
if(abs(vectorD(i)) > tolerance)
dst.row(i) /= vectorD(i);
@ -566,7 +570,7 @@ MatrixType LDLT<MatrixType,_UpLo>::reconstructedMatrix() const
// L^* P
res = matrixU() * res;
// D(L^*P)
res = vectorD().asDiagonal() * res;
res = vectorD().real().asDiagonal() * res;
// L(DL^*P)
res = matrixL() * res;
// P^T (LDL^*P)

23
gtsam/3rdparty/Eigen/Eigen/src/CholmodSupport/CholmodSupport.h vendored
View File

@ -58,10 +58,12 @@ cholmod_sparse viewAsCholmod(SparseMatrix<_Scalar,_Options,_Index>& mat)
res.p = mat.outerIndexPtr();
res.i = mat.innerIndexPtr();
res.x = mat.valuePtr();
res.z = 0;
res.sorted = 1;
if(mat.isCompressed())
{
res.packed = 1;
res.nz = 0;
}
else
{
@ -170,6 +172,7 @@ class CholmodBase : internal::noncopyable
CholmodBase()
: m_cholmodFactor(0), m_info(Success), m_isInitialized(false)
{
m_shiftOffset[0] = m_shiftOffset[1] = RealScalar(0.0);
cholmod_start(&m_cholmod);
}
@ -241,7 +244,7 @@ class CholmodBase : internal::noncopyable
return internal::sparse_solve_retval<CholmodBase, Rhs>(*this, b.derived());
}
/** Performs a symbolic decomposition on the sparcity of \a matrix.
/** Performs a symbolic decomposition on the sparsity pattern of \a matrix.
*
* This function is particularly useful when solving for several problems having the same structure.
*
@ -265,7 +268,7 @@ class CholmodBase : internal::noncopyable
/** Performs a numeric decomposition of \a matrix
*
* The given matrix must has the same sparcity than the matrix on which the symbolic decomposition has been performed.
* The given matrix must have the same sparsity pattern as the matrix on which the symbolic decomposition has been performed.
*
* \sa analyzePattern()
*/
@ -302,7 +305,7 @@ class CholmodBase : internal::noncopyable
{
this->m_info = NumericalIssue;
}
// TODO optimize this copy by swapping when possible (be carreful with alignment, etc.)
// TODO optimize this copy by swapping when possible (be careful with alignment, etc.)
dest = Matrix<Scalar,Dest::RowsAtCompileTime,Dest::ColsAtCompileTime>::Map(reinterpret_cast<Scalar*>(x_cd->x),b.rows(),b.cols());
cholmod_free_dense(&x_cd, &m_cholmod);
}
@ -323,7 +326,7 @@ class CholmodBase : internal::noncopyable
{
this->m_info = NumericalIssue;
}
// TODO optimize this copy by swapping when possible (be carreful with alignment, etc.)
// TODO optimize this copy by swapping when possible (be careful with alignment, etc.)
dest = viewAsEigen<DestScalar,DestOptions,DestIndex>(*x_cs);
cholmod_free_sparse(&x_cs, &m_cholmod);
}
@ -365,8 +368,8 @@ class CholmodBase : internal::noncopyable
*
* This class allows to solve for A.X = B sparse linear problems via a simplicial LL^T Cholesky factorization
* using the Cholmod library.
* This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Thefore, it has little practical interest.
* The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
* This simplicial variant is equivalent to Eigen's built-in SimplicialLLT class. Therefore, it has little practical interest.
* The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
* X and B can be either dense or sparse.
*
* \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
@ -412,8 +415,8 @@ class CholmodSimplicialLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimpl
*
* This class allows to solve for A.X = B sparse linear problems via a simplicial LDL^T Cholesky factorization
* using the Cholmod library.
* This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Thefore, it has little practical interest.
* The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
* This simplicial variant is equivalent to Eigen's built-in SimplicialLDLT class. Therefore, it has little practical interest.
* The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
* X and B can be either dense or sparse.
*
* \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
@ -458,7 +461,7 @@ class CholmodSimplicialLDLT : public CholmodBase<_MatrixType, _UpLo, CholmodSimp
* This class allows to solve for A.X = B sparse linear problems via a supernodal LL^T Cholesky factorization
* using the Cholmod library.
* This supernodal variant performs best on dense enough problems, e.g., 3D FEM, or very high order 2D FEM.
* The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
* The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
* X and B can be either dense or sparse.
*
* \tparam _MatrixType the type of the sparse matrix A, it must be a SparseMatrix<>
@ -501,7 +504,7 @@ class CholmodSupernodalLLT : public CholmodBase<_MatrixType, _UpLo, CholmodSuper
* \brief A general Cholesky factorization and solver based on Cholmod
*
* This class allows to solve for A.X = B sparse linear problems via a LL^T or LDL^T Cholesky factorization
* using the Cholmod library. The sparse matrix A must be selfajoint and positive definite. The vectors or matrices
* using the Cholmod library. The sparse matrix A must be selfadjoint and positive definite. The vectors or matrices
* X and B can be either dense or sparse.
*
* This variant permits to change the underlying Cholesky method at runtime.

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

@ -210,7 +210,7 @@ class Array
: Base(other.derived().rows() * other.derived().cols(), other.derived().rows(), other.derived().cols())
{
Base::_check_template_params();
Base::resize(other.rows(), other.cols());
Base::_resize_to_match(other);
*this = other;
}

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

@ -81,7 +81,7 @@ struct traits<Block<XprType, BlockRows, BlockCols, InnerPanel> > : traits<XprTyp
&& (InnerStrideAtCompileTime == 1)
? PacketAccessBit : 0,
MaskAlignedBit = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16) == 0)) ? AlignedBit : 0,
FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (traits<XprType>::Flags&LinearAccessBit))) ? LinearAccessBit : 0,
FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
Flags0 = traits<XprType>::Flags & ( (HereditaryBits & ~RowMajorBit) |

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