diff --git a/.cproject b/.cproject
index d081a750a6..6fe436adcc 100644
--- a/.cproject
+++ b/.cproject
@@ -510,34 +510,26 @@
true
true
-
- make
- -j5
- SFMExampleExpressions.run
- true
- true
- true
-
-
+
make
-j5
- Pose2SLAMExampleExpressions.run
+ testInvDepthCamera3.run
true
true
true
-
+
make
-j5
- testInvDepthCamera3.run
+ testTriangulation.run
true
true
true
-
+
make
- -j5
- testTriangulation.run
+ -j4
+ testEvent.run
true
true
true
@@ -856,6 +848,22 @@
true
true
+
+ make
+ -j4
+ testSmartStereoProjectionPoseFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testTOAFactor.run
+ true
+ true
+ true
+
make
-j5
@@ -960,38 +968,6 @@
true
true
-
- make
- -j5
- timeCameraExpression.run
- true
- true
- true
-
-
- make
- -j5
- timeOneCameraExpression.run
- true
- true
- true
-
-
- make
- -j5
- timeSFMExpressions.run
- true
- true
- true
-
-
- make
- -j5
- timeAdaptAutoDiff.run
- true
- true
- true
-
make
-j5
@@ -1583,6 +1559,14 @@
false
true
+
+ make
+ -j4
+ testLabeledSymbol.run
+ true
+ true
+ true
+
make
-j2
@@ -1994,6 +1978,14 @@
true
true
+
+ make
+ -j2 VERBOSE=1
+ check.navigation
+ true
+ false
+ true
+
make
-j2
@@ -2177,6 +2169,14 @@
true
true
+
+ make
+ -j4
+ testSO3.run
+ true
+ true
+ true
+
make
-j2
@@ -2537,6 +2537,14 @@
true
true
+
+ make
+ -j4
+ testOptionalJacobian.run
+ true
+ true
+ true
+
make
-j5
@@ -2766,14 +2774,6 @@
true
true
-
- make
- -j5
- testExpressionFactor.run
- true
- true
- true
-
make
-j5
@@ -2782,30 +2782,6 @@
true
true
-
- make
- -j5
- testAdaptAutoDiff.run
- true
- true
- true
-
-
- make
- -j5
- testCallRecord.run
- true
- true
- true
-
-
- make
- -j4
- testBasisDecompositions.run
- true
- true
- true
-
make
-j4
@@ -2950,6 +2926,14 @@
true
true
+
+ make
+ -j4
+ testRangeFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -3126,6 +3110,22 @@
true
true
+
+ make
+ -j4
+ Pose2SLAMExampleExpressions.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ SFMExampleExpressions.run
+ true
+ true
+ true
+
make
-j5
@@ -3174,6 +3174,30 @@
true
true
+
+ make
+ -j4
+ testAdaptAutoDiff.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testCallRecord.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ testExpressionFactor.run
+ true
+ true
+ true
+
make
-j4
@@ -3222,6 +3246,46 @@
true
true
+
+ make
+ -j4
+ timeAdaptAutoDiff.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ timeCameraExpression.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ timeOneCameraExpression.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ timeSFMExpressions.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ timeIncremental.run
+ true
+ true
+ true
+
make
-j2
diff --git a/.gitignore b/.gitignore
index e583e3ab08..d46bddd10f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,8 +1,8 @@
/build*
-/doc*
*.pyc
*.DS_Store
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
/examples/Data/pose2example-rewritten.txt
/examples/Data/pose3example-rewritten.txt
-/.settings/
+*.txt.user
+*.txt.user.6d59f0c
diff --git a/.settings/.gitignore b/.settings/.gitignore
new file mode 100644
index 0000000000..faa6d29912
--- /dev/null
+++ b/.settings/.gitignore
@@ -0,0 +1 @@
+/org.eclipse.cdt.codan.core.prefs
diff --git a/.settings/org.eclipse.cdt.core.prefs b/.settings/org.eclipse.cdt.core.prefs
new file mode 100644
index 0000000000..b700d0a818
--- /dev/null
+++ b/.settings/org.eclipse.cdt.core.prefs
@@ -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
diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h
index 3ea6a63185..ff1f1b692f 100644
--- a/CppUnitLite/Test.h
+++ b/CppUnitLite/Test.h
@@ -80,6 +80,12 @@ class Test
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
diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md
new file mode 100644
index 0000000000..3c10451be4
--- /dev/null
+++ b/GTSAM-Concepts.md
@@ -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`.
+ * `ChartJacobian`, a typedef for `OptionalJacobian`.
+ * `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::getDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time.
+ * `v = traits::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::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::Compose(p,q)`, where *p*, *q*, and *r* are elements of the manifold.
+ * `q = traits::Inverse(p)`, where *p* and*q* are elements of the manifold.
+ * `r = traits::Between(p,q)`, where *p*, *q*, and *r* are elements of the manifold.
+* static members:
+ * `traits::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::Identity` -- The identity element for this group stored as a static const.
+ * `traits::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`:
+
+* `r = traits::Compose(p,q,Hq,Hp)`
+* `q = traits::Inverse(p,Hp)`
+* `r = traits::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::Logmap(p,Hp)`, the log map, with optional Jacobian
+* `p = traits::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::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)
+
+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::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::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::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.
diff --git a/README.md b/README.md
index 623b1ff326..679af5a2fb 100644
--- a/README.md
+++ b/README.md
@@ -40,10 +40,12 @@ Optional prerequisites - used automatically if findable by CMake:
Additional Information
----------------------
-Read about important [`GTSAM-Concepts`] here.
+Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
-See the [`INSTALL`] file for more detailed installation instructions.
+See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
-GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
+GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
-Please see the [`examples/`](examples) directory and the [`USAGE`] file for examples on how to use GTSAM.
\ No newline at end of file
+Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
+
+GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).
\ No newline at end of file
diff --git a/THANKS b/THANKS
index 0d5d9db30b..9c06a5d287 100644
--- a/THANKS
+++ b/THANKS
@@ -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
diff --git a/USAGE b/USAGE.md
similarity index 57%
rename from USAGE
rename to USAGE.md
index a41b71045b..0493db6809 100644
--- a/USAGE
+++ b/USAGE.md
@@ -1,6 +1,5 @@
USAGE - Georgia Tech Smoothing and Mapping library
----------------------------------------------------
-
+===================================
What is this file?
This file explains how to make use of the library for common SLAM tasks,
@@ -34,18 +33,12 @@ The GTSAM library has three primary components necessary for the construction
of factor graph representation and optimization which users will need to
adapt to their particular problem.
-FactorGraph:
- A factor graph contains a set of variables to solve for (i.e., robot poses,
- landmark poses, etc.) and a set of constraints between these variables, which
- make up factors.
-Values:
- Values is a single object containing labeled values for all of the
- variables. Currently, all variables are labeled with strings, but the type
- or organization of the variables can change
-Factors:
- A nonlinear factor expresses a constraint between variables, which in the
- SLAM example, is a measurement such as a visual reading on a landmark or
- odometry.
+* FactorGraph:
+ A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors.
+* Values:
+ Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change
+* Factors:
+ A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry.
The library is organized according to the following directory structure:
@@ -59,23 +52,3 @@ The library is organized according to the following directory structure:
-VSLAM Example
----------------------------------------------------
-The visual slam example shows a full implementation of a slam system. The example contains
-derived versions of NonlinearFactor, NonlinearFactorGraph, in classes visualSLAM::ProjectionFactor,
-visualSLAM::Graph, respectively. The values for the system are stored in the generic
-Values structure. For definitions and interface, see gtsam/slam/visualSLAM.h.
-
-The clearest example of the use of the graph to find a solution is in
-testVSLAM. The basic process for using graphs is as follows (and can be seen in
-the test):
- - Create a NonlinearFactorGraph object (visualSLAM::Graph)
- - Add factors to the graph (note the use of Boost.shared_ptr here) (visualSLAM::ProjectionFactor)
- - Create an initial configuration (Values)
- - Create an elimination ordering of variables (this must include all variables)
- - Create and initialize a NonlinearOptimizer object (Note that this is a generic
- algorithm that does not need to be derived for a particular problem)
- - Call optimization functions with the optimizer to optimize the graph
- - Extract an updated values from the optimizer
-
-
\ No newline at end of file
diff --git a/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake
index 2f11df94e8..1bead58d89 100644
--- a/cmake/GtsamBuildTypes.cmake
+++ b/cmake/GtsamBuildTypes.cmake
@@ -54,7 +54,7 @@ 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")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
endif()
diff --git a/doc/Mathematica/dexpInvL_SE2.nb b/doc/Mathematica/dexpInvL_SE2.nb
new file mode 100644
index 0000000000..d159327685
--- /dev/null
+++ b/doc/Mathematica/dexpInvL_SE2.nb
@@ -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 *)
diff --git a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp
similarity index 82%
rename from gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp
rename to examples/Pose2SLAMExampleExpressions.cpp
index 3a3b97b776..92f94c3f31 100644
--- a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp
+++ b/examples/Pose2SLAMExampleExpressions.cpp
@@ -18,8 +18,8 @@
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
-#include
-#include
+#include
+#include
// Header order is close to far
#include
@@ -35,26 +35,26 @@ using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
- NonlinearFactorGraph graph;
+ ExpressionFactorGraph graph;
// Create Expressions for unknowns
Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
- graph.push_back(ExpressionFactor(priorNoise, Pose2(0, 0, 0), x1));
+ graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
- graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2)));
- graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3)));
- graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4)));
- graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x4,x5)));
+ graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model);
+ graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
+ graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
+ graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
- graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x5,x2)));
+ graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
diff --git a/gtsam_unstable/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp
similarity index 64%
rename from gtsam_unstable/examples/SFMExampleExpressions.cpp
rename to examples/SFMExampleExpressions.cpp
index 941f3dcb8d..e9c9e920d3 100644
--- a/gtsam_unstable/examples/SFMExampleExpressions.cpp
+++ b/examples/SFMExampleExpressions.cpp
@@ -23,15 +23,12 @@
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
-#include
-#include
+#include
+#include
// Header order is close to far
#include
-#include
-#include
#include
-#include
#include
#include
#include
@@ -40,27 +37,29 @@
using namespace std;
using namespace gtsam;
+using namespace noiseModel;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
- noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+ Isotropic::shared_ptr measurementNoise = Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
vector points = createPoints();
vector poses = createPoses();
// Create a factor graph
- NonlinearFactorGraph graph;
+ ExpressionFactorGraph graph;
// Specify uncertainty on first pose prior
- noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
+ Vector6 sigmas; sigmas << Vector3(0.3,0.3,0.3), Vector3(0.1,0.1,0.1);
+ Diagonal::shared_ptr poseNoise = Diagonal::Sigmas(sigmas);
// Here we don't use a PriorFactor but directly the ExpressionFactor class
- // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
+ // x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
Pose3_ x0('x',0);
- graph.push_back(ExpressionFactor(poseNoise, poses[0], x0));
+ graph.addExpressionFactor(x0, poses[0], poseNoise);
// We create a constant Expression for the calibration here
Cal3_S2_ cK(K);
@@ -74,25 +73,26 @@ int main(int argc, char* argv[]) {
// Below an expression for the prediction of the measurement:
Point3_ p('l', j);
Point2_ prediction = uncalibrate(cK, project(transform_to(x, p)));
- // Again, here we use a ExpressionFactor
- graph.push_back(ExpressionFactor(measurementNoise, measurement, prediction));
+ // Again, here we use an ExpressionFactor
+ graph.addExpressionFactor(prediction, measurement, measurementNoise);
}
}
// Add prior on first point to constrain scale, again with ExpressionFactor
- noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
- graph.push_back(ExpressionFactor(pointNoise, points[0], Point3_('l', 0)));
+ Isotropic::shared_ptr pointNoise = Isotropic::Sigma(3, 0.1);
+ graph.addExpressionFactor(Point3_('l', 0), points[0], pointNoise);
// Create perturbed initial
- Values initialEstimate;
+ Values initial;
+ Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
- initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+ initial.insert(Symbol('x', i), poses[i].compose(delta));
for (size_t j = 0; j < points.size(); ++j)
- initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
- cout << "initial error = " << graph.error(initialEstimate) << endl;
+ initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
+ cout << "initial error = " << graph.error(initial) << endl;
/* Optimize the graph and print results */
- Values result = DoglegOptimizer(graph, initialEstimate).optimize();
+ Values result = DoglegOptimizer(graph, initial).optimize();
cout << "final error = " << graph.error(result) << endl;
return 0;
diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp
index 3dd978ee33..923b0b9de8 100644
--- a/examples/SolverComparer.cpp
+++ b/examples/SolverComparer.cpp
@@ -72,22 +72,22 @@ typedef NoiseModelFactor1 NM1;
typedef NoiseModelFactor2 NM2;
typedef BearingRangeFactor 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);
-BOOST_CLASS_EXPORT(PriorFactor);
-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);
+//GTSAM_VALUE_EXPORT(PriorFactor);
+//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)
@@ -295,7 +295,7 @@ void runIncremental()
NonlinearFactorGraph newFactors;
Values newVariables;
- newFactors.push_back(boost::make_shared >(firstPose, Pose(), noiseModel::Unit::Create(Pose::Dim())));
+ newFactors.push_back(boost::make_shared >(firstPose, Pose(), noiseModel::Unit::Create(3)));
newVariables.insert(firstPose, Pose());
isam2.update(newFactors, newVariables);
@@ -474,7 +474,7 @@ void runBatch()
cout << "Creating batch optimizer..." << endl;
NonlinearFactorGraph measurements = datasetMeasurements;
- measurements.push_back(boost::make_shared >(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
+ measurements.push_back(boost::make_shared >(0, Pose(), noiseModel::Unit::Create(3)));
gttic_(Create_optimizer);
GaussNewtonParams params;
diff --git a/gtsam.h b/gtsam.h
index 96d51117ae..1fbc0f907a 100644
--- a/gtsam.h
+++ b/gtsam.h
@@ -156,12 +156,6 @@ virtual class Value {
size_t dim() const;
};
-class Vector3 {
- Vector3(Vector v);
-};
-class Vector6 {
- Vector6(Vector v);
-};
#include
class LieScalar {
// Standard constructors
@@ -276,8 +270,6 @@ class Point2 {
gtsam::Point2 between(const gtsam::Point2& p2) const;
// Manifold
- static size_t Dim();
- size_t dim() const;
gtsam::Point2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point2& p) const;
@@ -348,8 +340,6 @@ class Point3 {
gtsam::Point3 between(const gtsam::Point3& p2) const;
// Manifold
- static size_t Dim();
- size_t dim() const;
gtsam::Point3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point3& p) const;
@@ -386,8 +376,6 @@ class Rot2 {
gtsam::Rot2 between(const gtsam::Rot2& p2) const;
// Manifold
- static size_t Dim();
- size_t dim() const;
gtsam::Rot2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot2& p) const;
@@ -439,8 +427,6 @@ class Rot3 {
gtsam::Rot3 between(const gtsam::Rot3& p2) const;
// Manifold
- static size_t Dim();
- size_t dim() const;
//gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const;
@@ -488,8 +474,6 @@ class Pose2 {
gtsam::Pose2 between(const gtsam::Pose2& p2) const;
// Manifold
- static size_t Dim();
- size_t dim() const;
gtsam::Pose2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Pose2& p) const;
@@ -537,10 +521,7 @@ class Pose3 {
gtsam::Pose3 between(const gtsam::Pose3& p2) const;
// Manifold
- static size_t Dim();
- size_t dim() const;
gtsam::Pose3 retract(Vector v) const;
- gtsam::Pose3 retractFirstOrder(Vector v) const;
Vector localCoordinates(const gtsam::Pose3& T2) const;
// Lie Group
@@ -758,10 +739,6 @@ class CalibratedCamera {
gtsam::CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
- // Group
- gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
- gtsam::CalibratedCamera inverse() const;
-
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
@@ -1234,6 +1211,7 @@ class VectorValues {
#include
virtual class GaussianFactor {
+ gtsam::KeyVector keys() const;
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
@@ -1657,6 +1635,7 @@ class NonlinearFactorGraph {
void push_back(gtsam::NonlinearFactor* factor);
void add(gtsam::NonlinearFactor* factor);
bool exists(size_t idx) const;
+ gtsam::KeySet keys() const;
// NonlinearFactorGraph
double error(const gtsam::Values& values) const;
@@ -1727,6 +1706,7 @@ class Values {
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
+
void insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
@@ -1737,10 +1717,14 @@ class Values {
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
+ void insert(size_t j, const gtsam::SimpleCamera& t);
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
+ // Fixed size version
+ void insertFixed(size_t j, Vector t, size_t n);
+
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
@@ -1757,6 +1741,16 @@ class Values {
template
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
@@ -2153,7 +2147,7 @@ class NonlinearISAM {
#include
#include
-template
+template
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@@ -2198,10 +2192,14 @@ typedef gtsam::RangeFactor RangeFactorPosePoint2;
typedef gtsam::RangeFactor RangeFactorPosePoint3;
typedef gtsam::RangeFactor RangeFactorPose2;
typedef gtsam::RangeFactor RangeFactorPose3;
-typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint;
-typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint;
-typedef gtsam::RangeFactor RangeFactorCalibratedCamera;
-typedef gtsam::RangeFactor 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 RangeFactorCalibratedCameraPoint;
+//typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint;
+//typedef gtsam::RangeFactor RangeFactorCalibratedCamera;
+//typedef gtsam::RangeFactor RangeFactorSimpleCamera;
#include
@@ -2373,8 +2371,6 @@ class ConstantBias {
gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
// Manifold
- static size_t Dim();
- size_t dim() const;
gtsam::imuBias::ConstantBias retract(Vector v) const;
Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
@@ -2393,25 +2389,24 @@ class ConstantBias {
}///\namespace imuBias
#include
-class PoseVelocity{
- PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity);
+class PoseVelocityBias{
+ PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
- ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
+ // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
- Matrix measurementCovariance() const;
- Matrix deltaRij() const;
double deltaTij() const;
+ Matrix deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
- Vector biasHat() const;
+ Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
@@ -2419,10 +2414,11 @@ class ImuFactorPreintegratedMeasurements {
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
-
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+ gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
+ Vector gravity, Vector omegaCoriolis) const;
};
virtual class ImuFactor : gtsam::NonlinearFactor {
@@ -2433,54 +2429,9 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
- gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
- const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
- Vector gravity, Vector omegaCoriolis) const;
-};
-
-#include
-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 measurementCovariance() const;
- 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
-class PoseVelocityBias{
- PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
- };
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
@@ -2500,39 +2451,74 @@ class CombinedImuFactorPreintegratedMeasurements {
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
- CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
+ // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
- // Standard Interface
- void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
- void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
-
- Matrix measurementCovariance() const;
- Matrix deltaRij() const;
double deltaTij() const;
+ Matrix deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
- Vector biasHat() const;
+ Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
- Matrix PreintMeasCov() const;
+ Matrix preintMeasCov() const;
+
+ // Standard Interface
+ void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
+ void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
+ gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
+ Vector gravity, Vector omegaCoriolis) const;
};
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
-
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
- gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i,
- const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
- Vector gravity, Vector omegaCoriolis);
+};
+
+#include
+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
diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt
index fd9c7eaf7e..6ba17787fb 100644
--- a/gtsam/3rdparty/metis/CMakeLists.txt
+++ b/gtsam/3rdparty/metis/CMakeLists.txt
@@ -2,14 +2,14 @@ cmake_minimum_required(VERSION 2.8)
project(METIS)
# Add flags for currect directory and below
-if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
add_definitions(-Wno-unused-variable)
if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0)
add_definitions(-Wno-sometimes-uninitialized)
endif()
endif()
-if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC"))
+if(NOT (${CMAKE_C_COMPILER_ID} MATCHES "MSVC" OR ${CMAKE_CXX_COMPILER_ID} MATCHES "MSVC"))
#add_definitions(-Wno-unknown-pragmas)
endif()
diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h
deleted file mode 100644
index 3cc6a041c5..0000000000
--- a/gtsam/base/ChartValue.h
+++ /dev/null
@@ -1,221 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 ChartValue.h
- * @brief
- * @date October, 2014
- * @author Michael Bosse, Abel Gawel, Renaud Dube
- * based on DerivedValue.h by Duy Nguyen Ta
- */
-
-#pragma once
-
-#include
-#include
-#include
-
-//////////////////
-// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR
-#include
-
-#ifdef min
-#undef min
-#endif
-
-#ifdef max
-#undef max
-#endif
-
-#ifdef ERROR
-#undef ERROR
-#endif
-//////////////////
-
-namespace gtsam {
-
-/**
- * ChartValue is derived from GenericValue and Chart so that
- * Chart can be zero sized (as in DefaultChart)
- * if the Chart is a member variable then it won't ever be zero sized.
- */
-template >
-class ChartValue: public GenericValue, public Chart_ {
-
- BOOST_CONCEPT_ASSERT((ChartConcept));
-
-public:
-
- typedef T type;
- typedef Chart_ Chart;
-
-public:
-
- /// Default Constructor. TODO might not make sense for some types
- ChartValue() :
- GenericValue(T()) {
- }
-
- /// Construct froma value
- ChartValue(const T& value) :
- GenericValue(value) {
- }
-
- /// Construct from a value and initialize the chart
- template
- ChartValue(const T& value, C chart_initializer) :
- GenericValue(value), Chart(chart_initializer) {
- }
-
- /// Destructor
- virtual ~ChartValue() {
- }
-
- /**
- * Create a duplicate object returned as a pointer to the generic Value interface.
- * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
- * The result must be deleted with Value::deallocate_, not with the 'delete' operator.
- */
- virtual Value* clone_() const {
- void *place = boost::singleton_pool::malloc();
- ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in
- return ptr;
- }
-
- /**
- * Destroy and deallocate this object, only if it was originally allocated using clone_().
- */
- virtual void deallocate_() const {
- this->~ChartValue(); // Virtual destructor cleans up the derived object
- boost::singleton_pool::free((void*) this); // Release memory from pool
- }
-
- /**
- * Clone this value (normal clone on the heap, delete with 'delete' operator)
- */
- virtual boost::shared_ptr clone() const {
- return boost::make_shared(*this);
- }
-
- /// Chart Value interface version of retract
- virtual Value* retract_(const Vector& delta) const {
- // Call retract on the derived class using the retract trait function
- const T retractResult = Chart::retract(GenericValue::value(), delta);
-
- // Create a Value pointer copy of the result
- void* resultAsValuePlace =
- boost::singleton_pool::malloc();
- Value* resultAsValue = new (resultAsValuePlace) ChartValue(retractResult,
- static_cast(*this));
-
- // Return the pointer to the Value base class
- return resultAsValue;
- }
-
- /// Generic Value interface version of localCoordinates
- virtual Vector localCoordinates_(const Value& value2) const {
- // Cast the base class Value pointer to a templated generic class pointer
- const GenericValue& genericValue2 =
- static_cast&>(value2);
-
- // Return the result of calling localCoordinates trait on the derived class
- return Chart::local(GenericValue::value(), genericValue2.value());
- }
-
- /// Non-virtual version of retract
- ChartValue retract(const Vector& delta) const {
- return ChartValue(Chart::retract(GenericValue::value(), delta),
- static_cast(*this));
- }
-
- /// Non-virtual version of localCoordinates
- Vector localCoordinates(const ChartValue& value2) const {
- return localCoordinates_(value2);
- }
-
- /// Return run-time dimensionality
- virtual size_t dim() const {
- // need functional form here since the dimension may be dynamic
- return Chart::getDimension(GenericValue::value());
- }
-
- /// Assignment operator
- virtual Value& operator=(const Value& rhs) {
- // Cast the base class Value pointer to a derived class pointer
- const ChartValue& derivedRhs = static_cast(rhs);
-
- // Do the assignment and return the result
- *this = ChartValue(derivedRhs); // calls copy constructor
- return *this;
- }
-
-protected:
-
- // implicit assignment operator for (const ChartValue& rhs) works fine here
- /// Assignment operator, protected because only the Value or DERIVED
- /// assignment operators should be used.
- // DerivedValue& operator=(const DerivedValue& rhs) {
- // // Nothing to do, do not call base class assignment operator
- // return *this;
- // }
-
-private:
-
- /// Fake Tag struct for singleton pool allocator. In fact, it is never used!
- struct PoolTag {
- };
-
-private:
-
- /** Serialization function */
- friend class boost::serialization::access;
- template
- void serialize(ARCHIVE & ar, const unsigned int version) {
- // ar & boost::serialization::make_nvp("value",);
- // todo: implement a serialization for charts
- ar
- & boost::serialization::make_nvp("GenericValue",
- boost::serialization::base_object >(*this));
- }
-
-};
-
-// Define
-namespace traits {
-
-/// The dimension of a ChartValue is the dimension of the chart
-template
-struct dimension > : public dimension {
- // TODO Frank thinks dimension is a property of type, chart should conform
-};
-
-} // \ traits
-
-/// Get the chart from a Value
-template
-const Chart& Value::getChart() const {
- return dynamic_cast(*this);
-}
-
-/// Convenience function that can be used to make an expression to convert a value to a chart
-template
-ChartValue convertToChartValue(const T& value,
- boost::optional<
- Eigen::Matrix::value,
- traits::dimension::value>&> H = boost::none) {
- if (H) {
- *H = Eigen::Matrix::value,
- traits::dimension::value>::Identity();
- }
- return ChartValue(value);
-}
-
-} /* namespace gtsam */
diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h
index 6c109675d5..c67f7dd61e 100644
--- a/gtsam/base/GenericValue.h
+++ b/gtsam/base/GenericValue.h
@@ -19,78 +19,18 @@
#pragma once
-#include
+#include
#include
+
+#include
+#include
+
#include
#include
+#include // operator typeid
namespace gtsam {
-// To play as a GenericValue, we need the following traits
-namespace traits {
-
-// trait to wrap the default equals of types
-template
-struct equals {
- typedef T type;
- typedef bool result_type;
- bool operator()(const T& a, const T& b, double tol) {
- return a.equals(b, tol);
- }
-};
-
-// trait to wrap the default print of types
-template
-struct print {
- typedef T type;
- typedef void result_type;
- void operator()(const T& obj, const std::string& str) {
- obj.print(str);
- }
-};
-
-// equals for scalars
-template<>
-struct equals {
- typedef double type;
- typedef bool result_type;
- bool operator()(double a, double b, double tol) {
- return std::abs(a - b) <= tol;
- }
-};
-
-// print for scalars
-template<>
-struct print {
- typedef double type;
- typedef void result_type;
- void operator()(double a, const std::string& str) {
- std::cout << str << ": " << a << std::endl;
- }
-};
-
-// equals for Matrix types
-template
-struct equals > {
- typedef Eigen::Matrix type;
- typedef bool result_type;
- bool operator()(const type& A, const type& B, double tol) {
- return equal_with_abs_tol(A, B, tol);
- }
-};
-
-// print for Matrix types
-template
-struct print > {
- typedef Eigen::Matrix type;
- typedef void result_type;
- void operator()(const type& A, const std::string& str) {
- std::cout << str << ": " << A << std::endl;
- }
-};
-
-}
-
/**
* Wraps any type T so it can play as a Value
*/
@@ -106,6 +46,8 @@ class GenericValue: public Value {
T value_; ///< The wrapped value
public:
+ // Only needed for serialization.
+ GenericValue(){}
/// Construct from value
GenericValue(const T& value) :
@@ -131,31 +73,124 @@ class GenericValue: public Value {
// Cast the base class Value pointer to a templated generic class pointer
const GenericValue& genericValue2 = static_cast(p);
// Return the result of using the equals traits for the derived class
- return traits::equals()(this->value_, genericValue2.value_, tol);
+ return traits::Equals(this->value_, genericValue2.value_, tol);
}
/// non virtual equals function, uses traits
bool equals(const GenericValue &other, double tol = 1e-9) const {
- return traits::equals()(this->value(), other.value(), tol);
+ return traits::Equals(this->value(), other.value(), tol);
}
/// Virtual print function, uses traits
virtual void print(const std::string& str) const {
- traits::print()(value_, str);
+ std::cout << "(" << typeid(T).name() << ") ";
+ traits::Print(value_, str);
}
- // Serialization below:
-
- friend class boost::serialization::access;
- template
- void serialize(ARCHIVE & ar, const unsigned int version) {
- ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
- ar & BOOST_SERIALIZATION_NVP(value_);
- }
-
-protected:
-
- // Assignment operator for this class not needed since GenericValue is an abstract class
+ /**
+ * Create a duplicate object returned as a pointer to the generic Value interface.
+ * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
+ * The result must be deleted with Value::deallocate_, not with the 'delete' operator.
+ */
+ virtual Value* clone_() const {
+ void *place = boost::singleton_pool::malloc();
+ GenericValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in
+ return ptr;
+ }
+
+ /**
+ * Destroy and deallocate this object, only if it was originally allocated using clone_().
+ */
+ virtual void deallocate_() const {
+ this->~GenericValue(); // Virtual destructor cleans up the derived object
+ boost::singleton_pool::free((void*) this); // Release memory from pool
+ }
+
+ /**
+ * Clone this value (normal clone on the heap, delete with 'delete' operator)
+ */
+ virtual boost::shared_ptr clone() const {
+ return boost::make_shared(*this);
+ }
+
+ /// Generic Value interface version of retract
+ virtual Value* retract_(const Vector& delta) const {
+ // Call retract on the derived class using the retract trait function
+ const T retractResult = traits::Retract(GenericValue::value(), delta);
+
+ // Create a Value pointer copy of the result
+ void* resultAsValuePlace =
+ boost::singleton_pool::malloc();
+ Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult);
+
+ // Return the pointer to the Value base class
+ return resultAsValue;
+ }
+
+ /// Generic Value interface version of localCoordinates
+ virtual Vector localCoordinates_(const Value& value2) const {
+ // Cast the base class Value pointer to a templated generic class pointer
+ const GenericValue& genericValue2 =
+ static_cast&>(value2);
+
+ // Return the result of calling localCoordinates trait on the derived class
+ return traits::Local(GenericValue::value(), genericValue2.value());
+ }
+
+ /// Non-virtual version of retract
+ GenericValue retract(const Vector& delta) const {
+ return GenericValue(traits::Retract(GenericValue::value(), delta));
+ }
+
+ /// Non-virtual version of localCoordinates
+ Vector localCoordinates(const GenericValue& value2) const {
+ return localCoordinates_(value2);
+ }
+
+ /// Return run-time dimensionality
+ virtual size_t dim() const {
+ return traits::GetDimension(value_);
+ }
+
+ /// Assignment operator
+ virtual Value& operator=(const Value& rhs) {
+ // Cast the base class Value pointer to a derived class pointer
+ const GenericValue& derivedRhs = static_cast(rhs);
+
+ // Do the assignment and return the result
+ *this = GenericValue(derivedRhs); // calls copy constructor
+ return *this;
+ }
+
+ protected:
+
+ // implicit assignment operator for (const GenericValue& rhs) works fine here
+ /// Assignment operator, protected because only the Value or DERIVED
+ /// assignment operators should be used.
+ // DerivedValue& operator=(const DerivedValue& rhs) {
+ // // Nothing to do, do not call base class assignment operator
+ // return *this;
+ // }
+
+ private:
+
+ /// Fake Tag struct for singleton pool allocator. In fact, it is never used!
+ struct PoolTag {
+ };
+
+ private:
+
+ /** Serialization function */
+ friend class boost::serialization::access;
+ template
+ void serialize(ARCHIVE & ar, const unsigned int version) {
+ ar & boost::serialization::make_nvp("GenericValue",
+ boost::serialization::base_object(*this));
+ ar & boost::serialization::make_nvp("value", value_);
+ }
+
+/// use this macro instead of BOOST_CLASS_EXPORT for GenericValues
+#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(GenericValue)
};
diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h
index 5b2574ee3b..aec4b5f1c4 100644
--- a/gtsam/base/Group.h
+++ b/gtsam/base/Group.h
@@ -19,43 +19,93 @@
#pragma once
-#include
+#include
+#include
+#include
+#include
namespace gtsam {
+/// tag to assert a type is a group
+struct group_tag {};
+
+/// Group operator syntax flavors
+struct multiplicative_group_tag {};
+struct additive_group_tag {};
+
+template struct traits;
+
/**
- * This concept check enforces a Group structure on a variable type,
- * in which we require the existence of basic algebraic operations.
+ * Group Concept
*/
-template
-class GroupConcept {
+template
+class IsGroup {
+public:
+ typedef typename traits::structure_category structure_category_tag;
+ typedef typename traits::group_flavor flavor_tag;
+ //typedef typename traits::identity::value_type identity_value_type;
+
+ BOOST_CONCEPT_USAGE(IsGroup) {
+ BOOST_STATIC_ASSERT_MSG(
+ (boost::is_base_of::value),
+ "This type's structure_category trait does not assert it as a group (or derived)");
+ e = traits::Identity();
+ e = traits::Compose(g, h);
+ e = traits::Between(g, h);
+ e = traits::Inverse(g);
+ operator_usage(flavor);
+ // todo: how do we test the act concept? or do we even need to?
+ }
+
private:
- static T concept_check(const T& t) {
- /** assignment */
- T t2 = t;
+ void operator_usage(multiplicative_group_tag) {
+ e = g * h;
+ //e = -g; // todo this should work, but it is failing for Quaternions
+ }
+ void operator_usage(additive_group_tag) {
+ e = g + h;
+ e = h - g;
+ e = -g;
+ }
- /** identity */
- T identity = T::identity();
+ flavor_tag flavor;
+ G e, g, h;
+ bool b;
+};
- /** compose with another object */
- T compose_ret = identity.compose(t2);
+/// Check invariants
+template
+BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) //
+check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
+ G e = traits::Identity();
+ return traits::Equals(traits::Compose(a, traits::Inverse(a)), e, tol)
+ && traits::Equals(traits::Between(a, b), traits::Compose(traits::Inverse(a), b), tol)
+ && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol);
+}
- /** invert the object and yield a new one */
- T inverse_ret = compose_ret.inverse();
+/// Macro to add group traits, additive flavor
+#define GTSAM_ADDITIVE_GROUP(GROUP) \
+ typedef additive_group_tag group_flavor; \
+ static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \
+ static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \
+ static GROUP Inverse(const GROUP &g) { return -g;}
- return inverse_ret;
- }
-};
+/// Macro to add group traits, multiplicative flavor
+#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \
+ typedef additive_group_tag group_flavor; \
+ static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \
+ static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \
+ static GROUP Inverse(const GROUP &g) { return g.inverse();}
-} // \namespace gtsam
+} // \ namespace gtsam
/**
- * Macros for using the GroupConcept
+ * Macros for using the IsGroup
* - An instantiation for use inside unit tests
* - A typedef for use inside generic algorithms
*
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable
*/
-#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::GroupConcept;
-#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept _gtsam_GroupConcept_##T;
+#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup;
+#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup _gtsam_IsGroup_##T;
diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h
index 7a9d32249e..bf2056cc87 100644
--- a/gtsam/base/Lie.h
+++ b/gtsam/base/Lie.h
@@ -14,6 +14,9 @@
* @brief Base class and basic functions for Lie types
* @author Richard Roberts
* @author Alex Cunningham
+ * @author Frank Dellaert
+ * @author Mike Bosse
+ * @author Duy Nguyen Ta
*/
@@ -24,82 +27,235 @@
namespace gtsam {
+/// A CRTP helper class that implements Lie group methods
+/// Prerequisites: methods operator*, inverse, and AdjointMap, as well as a
+/// ChartAtOrigin struct that will be used to define the manifold Chart
+/// To use, simply derive, but also say "using LieGroup::inverse"
+/// For derivative math, see doc/math.pdf
+template
+struct LieGroup {
+
+ BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic,
+ "LieGroup not yet specialized for dynamically sized types.");
+
+ enum { dimension = N };
+ typedef OptionalJacobian ChartJacobian;
+ typedef Eigen::Matrix Jacobian;
+ typedef Eigen::Matrix TangentVector;
+
+ const Class & derived() const {
+ return static_cast(*this);
+ }
+
+ Class compose(const Class& g) const {
+ return derived() * g;
+ }
+
+ Class between(const Class& g) const {
+ return derived().inverse() * g;
+ }
+
+ Class compose(const Class& g, ChartJacobian H1,
+ ChartJacobian H2 = boost::none) const {
+ if (H1) *H1 = g.inverse().AdjointMap();
+ if (H2) *H2 = Eigen::Matrix::Identity();
+ return derived() * g;
+ }
+
+ Class between(const Class& g, ChartJacobian H1,
+ ChartJacobian H2 = boost::none) const {
+ Class result = derived().inverse() * g;
+ if (H1) *H1 = - result.inverse().AdjointMap();
+ if (H2) *H2 = Eigen::Matrix::Identity();
+ return result;
+ }
+
+ Class inverse(ChartJacobian H) const {
+ if (H) *H = - derived().AdjointMap();
+ return derived().inverse();
+ }
+
+ Class expmap(const TangentVector& v) const {
+ return compose(Class::Expmap(v));
+ }
+
+ TangentVector logmap(const Class& g) const {
+ return Class::Logmap(between(g));
+ }
+
+ static Class Retract(const TangentVector& v) {
+ return Class::ChartAtOrigin::Retract(v);
+ }
+
+ static TangentVector LocalCoordinates(const Class& g) {
+ return Class::ChartAtOrigin::Local(g);
+ }
+
+ static Class Retract(const TangentVector& v, ChartJacobian H) {
+ return Class::ChartAtOrigin::Retract(v,H);
+ }
+
+ static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) {
+ return Class::ChartAtOrigin::Local(g,H);
+ }
+
+ Class retract(const TangentVector& v) const {
+ return compose(Class::ChartAtOrigin::Retract(v));
+ }
+
+ TangentVector localCoordinates(const Class& g) const {
+ return Class::ChartAtOrigin::Local(between(g));
+ }
+
+ Class retract(const TangentVector& v, //
+ ChartJacobian H1, ChartJacobian H2 = boost::none) const {
+ Jacobian D_g_v;
+ Class g = Class::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
+ Class h = compose(g,H1,H2);
+ if (H2) *H2 = (*H2) * D_g_v;
+ return h;
+ }
+
+ TangentVector localCoordinates(const Class& g, //
+ ChartJacobian H1, ChartJacobian H2 = boost::none) const {
+ Class h = between(g,H1,H2);
+ Jacobian D_v_h;
+ TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
+ if (H1) *H1 = D_v_h * (*H1);
+ if (H2) *H2 = D_v_h * (*H2);
+ return v;
+ }
+
+};
+
+/// tag to assert a type is a Lie group
+struct lie_group_tag: public manifold_tag, public group_tag {};
+
+namespace internal {
+
+/// A helper class that implements the traits interface for GTSAM lie groups.
+/// To use this for your gtsam type, define:
+/// template<> struct traits : public internal::LieGroupTraits {};
+template
+struct LieGroupTraits : Testable {
+ typedef lie_group_tag structure_category;
+
+ /// @name Group
+ /// @{
+ typedef multiplicative_group_tag group_flavor;
+ static Class Identity() { return Class::identity();}
+ /// @}
+
+ /// @name Manifold
+ /// @{
+ typedef Class ManifoldType;
+ enum { dimension = Class::dimension };
+ typedef Eigen::Matrix TangentVector;
+ typedef OptionalJacobian ChartJacobian;
+
+ BOOST_STATIC_ASSERT_MSG(dimension != Eigen::Dynamic,
+ "LieGroupTraits not yet specialized for dynamically sized types.");
+
+ static int GetDimension(const Class&) {return dimension;}
+
+ static TangentVector Local(const Class& origin, const Class& other,
+ ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+ return origin.localCoordinates(other, Horigin, Hother);
+ }
+
+ static Class Retract(const Class& origin, const TangentVector& v,
+ ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
+ return origin.retract(v, Horigin, Hv);
+ }
+
+ /// @}
+
+ /// @name Lie Group
+ /// @{
+
+ static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
+ return Class::Logmap(m, Hm);
+ }
+
+ static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
+ return Class::Expmap(v, Hv);
+ }
+
+ static Class Compose(const Class& m1, const Class& m2, //
+ ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+ return m1.compose(m2, H1, H2);
+ }
+
+ static Class Between(const Class& m1, const Class& m2, //
+ ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+ return m1.between(m2, H1, H2);
+ }
+
+ static Class Inverse(const Class& m, //
+ ChartJacobian H = boost::none) {
+ return m.inverse(H);
+ }
+
+ /// @}
+
+};
+
+} // \ namepsace internal
+
/**
* These core global functions can be specialized by new Lie types
* for better performance.
*/
/** Compute l0 s.t. l2=l1*l0 */
-template
-inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);}
+template
+inline Class between_default(const Class& l1, const Class& l2) {
+ return l1.inverse().compose(l2);
+}
/** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */
-template
-inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));}
+template
+inline Vector logmap_default(const Class& l0, const Class& lp) {
+ return Class::Logmap(l0.between(lp));
+}
/** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
-template
-inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));}
+template
+inline Class expmap_default(const Class& t, const Vector& d) {
+ return t.compose(Class::Expmap(d));
+}
/**
- * Concept check class for Lie group type
- *
- * This concept check provides a specialization on the Manifold type,
- * in which the Manifolds represented require an algebra and group structure.
- * All Lie types must also be a Manifold.
- *
- * The necessary functions to implement for Lie are defined
- * below with additional details as to the interface. The
- * concept checking function in class Lie will check whether or not
- * the function exists and throw compile-time errors.
- *
- * The exponential map is a specific retraction for Lie groups,
- * which maps the tangent space in canonical coordinates back to
- * the underlying manifold. Because we can enforce a group structure,
- * a retraction of delta v, with tangent space centered at x1 can be performed
- * as x2 = x1.compose(Expmap(v)).
- *
- * Expmap around identity
- * static T Expmap(const Vector& v);
- *
- * Logmap around identity
- * static Vector Logmap(const T& p);
- *
- * Compute l0 s.t. l2=l1*l0, where (*this) is l1
- * A default implementation of between(*this, lp) is available:
- * between_default()
- * T between(const T& l2) const;
- *
- * By convention, we use capital letters to designate a static function
- *
- * @tparam T is a Lie type, like Point2, Pose3, etc.
+ * Lie Group Concept
*/
-template
-class LieConcept {
-private:
- /** concept checking function - implement the functions this demands */
- static T concept_check(const T& t) {
-
- /** assignment */
- T t2 = t;
+template
+class IsLieGroup: public IsGroup, public IsManifold {
+public:
+ typedef typename traits::structure_category structure_category_tag;
+ typedef typename traits::ManifoldType ManifoldType;
+ typedef typename traits::TangentVector TangentVector;
+ typedef typename traits::ChartJacobian ChartJacobian;
- /**
- * Returns dimensionality of the tangent space
- */
- size_t dim_ret = t.dim();
+ BOOST_CONCEPT_USAGE(IsLieGroup) {
+ BOOST_STATIC_ASSERT_MSG(
+ (boost::is_base_of::value),
+ "This type's trait does not assert it is a Lie group (or derived)");
- /** expmap around identity */
- T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret));
-
- /** Logmap around identity */
- Vector logmap_identity_ret = T::Logmap(t);
-
- /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */
- T between_ret = expmap_identity_ret.between(t2);
-
- return between_ret;
+ // group opertations with Jacobians
+ g = traits::Compose(g, h, Hg, Hh);
+ g = traits::Between(g, h, Hg, Hh);
+ g = traits::Inverse(g, Hg);
+ // log and exp map without Jacobians
+ g = traits::Expmap(v);
+ v = traits::Logmap(g);
+ // log and expnential map with Jacobians
+ g = traits::Expmap(v, Hg);
+ v = traits::Logmap(g, Hg);
}
-
+private:
+ T g, h;
+ TangentVector v;
+ ChartJacobian Hg, Hh;
};
/**
@@ -146,12 +302,5 @@ T expm(const Vector& x, int K=7) {
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable
*/
-#define GTSAM_CONCEPT_LIE_INST(T) \
- template class gtsam::ManifoldConcept; \
- template class gtsam::GroupConcept; \
- template class gtsam::LieConcept;
-
-#define GTSAM_CONCEPT_LIE_TYPE(T) \
- typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \
- typedef gtsam::GroupConcept