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

Merged in feature/tighteningTraits (pull request #64) 

Proposed Concepts Framework
release/4.3a0
Frank Dellaert 2014-12-28 13:10:03 +01:00
parent 75eab42636 a25b2f14d6
commit d9d43731d7
207 changed files with 4989 additions and 3380 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
.cproject
View File

@ -592,6 +592,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -599,6 +600,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -646,6 +648,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -653,6 +656,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -660,6 +664,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -675,6 +680,7 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1130,6 +1136,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1359,6 +1366,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1441,7 +1488,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1481,7 +1527,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1489,7 +1534,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1503,46 +1547,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1800,6 +1804,7 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1807,6 +1812,7 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1814,6 +1820,7 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1821,6 +1828,7 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2185,6 +2193,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSO3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSO3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2683,6 +2699,7 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2690,6 +2707,7 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2697,6 +2715,7 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3240,7 +3259,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

201
GTSAM-Concepts.md Normal file
View File

@ -0,0 +1,201 @@
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).
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
---------
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.
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.
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.

10
README.md
View File

@ -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.
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).

55
THANKS
View File

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

41
USAGE → USAGE.md
View File

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

32
examples/SolverComparer.cpp
View File

@ -72,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)

15
gtsam.h
View File

@ -270,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;
@ -342,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;
@ -380,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;
@ -433,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;
@ -482,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;
@ -531,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
@ -2384,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;

439
gtsam/base/ChartValue.h
View File

@ -1,221 +1,222 @@
/* ----------------------------------------------------------------------------
* 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
*/
///* ----------------------------------------------------------------------------
//
// * 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 <gtsam/base/concepts.h>
#include <gtsam/base/GenericValue.h>
#include <gtsam/base/Manifold.h>
#include <boost/make_shared.hpp>
//////////////////
// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR
#include <boost/pool/singleton_pool.hpp>
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif
#ifdef ERROR
#undef ERROR
#endif
//////////////////
namespace gtsam {
/**
* ChartValue is derived from GenericValue<T> and Chart so that
* Chart can be zero sized (as in DefaultChart<T>)
* if the Chart is a member variable then it won't ever be zero sized.
*/
template<class T, class Chart_ = DefaultChart<T> >
class ChartValue: public GenericValue<T>, public Chart_ {
BOOST_CONCEPT_ASSERT((ChartConcept<Chart_>));
public:
typedef T type;
typedef Chart_ Chart;
public:
/// Default Constructor. TODO might not make sense for some types
ChartValue() :
GenericValue<T>(T()) {
}
/// Construct froma value
ChartValue(const T& value) :
GenericValue<T>(value) {
}
/// Construct from a value and initialize the chart
template<typename C>
ChartValue(const T& value, C chart_initializer) :
GenericValue<T>(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<PoolTag, sizeof(ChartValue)>::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<PoolTag, sizeof(ChartValue)>::free((void*) this); // Release memory from pool
}
/**
* Clone this value (normal clone on the heap, delete with 'delete' operator)
*/
virtual boost::shared_ptr<Value> clone() const {
return boost::make_shared<ChartValue>(*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<T>::value(), delta);
// Create a Value pointer copy of the result
void* resultAsValuePlace =
boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc();
Value* resultAsValue = new (resultAsValuePlace) ChartValue(retractResult,
static_cast<const Chart&>(*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<T>& genericValue2 =
static_cast<const GenericValue<T>&>(value2);
// Return the result of calling localCoordinates trait on the derived class
return Chart::local(GenericValue<T>::value(), genericValue2.value());
}
/// Non-virtual version of retract
ChartValue retract(const Vector& delta) const {
return ChartValue(Chart::retract(GenericValue<T>::value(), delta),
static_cast<const Chart&>(*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<T>::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<const ChartValue&>(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<DERIVED>& operator=(const DerivedValue<DERIVED>& 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<class ARCHIVE>
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<GenericValue<T> >(*this));
}
};
// Define
namespace traits {
/// The dimension of a ChartValue is the dimension of the chart
template<typename T, typename Chart>
struct dimension<ChartValue<T, Chart> > : public dimension<Chart> {
// TODO Frank thinks dimension is a property of type, chart should conform
};
} // \ traits
/// Get the chart from a Value
template<typename Chart>
const Chart& Value::getChart() const {
return dynamic_cast<const Chart&>(*this);
}
/// Convenience function that can be used to make an expression to convert a value to a chart
template<typename T>
ChartValue<T> convertToChartValue(const T& value,
boost::optional<
Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<T>::value>&> H = boost::none) {
if (H) {
*H = Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<T>::value>::Identity();
}
return ChartValue<T>(value);
}
} /* namespace gtsam */
//#include <gtsam/base/Manifold.h>
//#include <boost/make_shared.hpp>
//
////////////////////
//// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR
//#include <boost/pool/singleton_pool.hpp>
//
//#ifdef min
//#undef min
//#endif
//
//#ifdef max
//#undef max
//#endif
//
//#ifdef ERROR
//#undef ERROR
//#endif
////////////////////
//
//namespace gtsam {
//
///**
// * ChartValue is derived from GenericValue<T> and Chart so that
// * Chart can be zero sized (as in DefaultChart<T>)
// * if the Chart is a member variable then it won't ever be zero sized.
// */
//template<class T, class Chart_ = DefaultChart<T> >
//class GenericValue: public GenericValue<T>, public Chart_ {
//
// BOOST_CONCEPT_ASSERT((ChartConcept<Chart_>));
//
//public:
//
// typedef T type;
// typedef Chart_ Chart;
//
//public:
//
// /// Default Constructor. TODO might not make sense for some types
// GenericValue() :
// GenericValue<T>(T()) {
// }
//
// /// Construct froma value
// GenericValue(const T& value) :
// GenericValue<T>(value) {
// }
//
// /// Construct from a value and initialize the chart
// template<typename C>
// GenericValue(const T& value, C chart_initializer) :
// GenericValue<T>(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<PoolTag, sizeof(GenericValue)>::malloc();
// ChartValue* 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->~ChartValue(); // Virtual destructor cleans up the derived object
// boost::singleton_pool<PoolTag, sizeof(ChartValue)>::free((void*) this); // Release memory from pool
// }
//
// /**
// * Clone this value (normal clone on the heap, delete with 'delete' operator)
// */
// virtual boost::shared_ptr<Value> clone() const {
// return boost::make_shared<ChartValue>(*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<T>::value(), delta);
//
// // Create a Value pointer copy of the result
// void* resultAsValuePlace =
// boost::singleton_pool<PoolTag, sizeof(GenericValue)>::malloc();
// Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult,
// static_cast<const Chart&>(*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<T>& genericValue2 =
// static_cast<const GenericValue<T>&>(value2);
//
// // Return the result of calling localCoordinates trait on the derived class
// return Chart::local(GenericValue<T>::value(), genericValue2.value());
// }
//
// /// Non-virtual version of retract
// GenericValue retract(const Vector& delta) const {
// return GenericValue(Chart::retract(GenericValue<T>::value(), delta),
// static_cast<const Chart&>(*this));
// }
//
// /// Non-virtual version of localCoordinates
// Vector localCoordinates(const GenericValue& 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<T>::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<const ChartValue&>(rhs);
//
// // Do the assignment and return the result
// *this = GenericValue(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<DERIVED>& operator=(const DerivedValue<DERIVED>& 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<class ARCHIVE>
// 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<GenericValue<T> >(*this));
// }
//
//};
//
//// Define
//namespace traits {
//
///// The dimension of a GenericValue is the dimension of the chart
//template<typename T, typename Chart>
//struct dimension<GenericValue<T, Chart> > : public dimension<Chart> {
// // TODO Frank thinks dimension is a property of type, chart should conform
//};
//
//} // \ traits
//
///// Get the chart from a Value
//template<typename Chart>
//const Chart& Value::getChart() const {
// return dynamic_cast<const Chart&>(*this);
//}
//
///// Convenience function that can be used to make an expression to convert a value to a chart
//template<typename T>
//GenericValue<T> convertToChartValue(const T& value,
// boost::optional<
// Eigen::Matrix<double, traits<T>::dimension,
// traits<T>::dimension>&> H = boost::none) {
// if (H) {
// *H = Eigen::Matrix<double, traits<T>::dimension,
// traits<T>::dimension>::Identity();
// }
// return GenericValue<T>(value);
//}
//
//} /* namespace gtsam */

189
gtsam/base/GenericValue.h
View File

@ -19,78 +19,17 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Value.h>
#include <boost/make_shared.hpp>
#include <boost/pool/pool_alloc.hpp>
#include <cmath>
#include <iostream>
namespace gtsam {
// To play as a GenericValue, we need the following traits
namespace traits {
// trait to wrap the default equals of types
template<typename T>
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<typename T>
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<double> {
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<double> {
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<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct equals<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> 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<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct print<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> 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 +45,8 @@ protected:
T value_; ///< The wrapped value
public:
// Only needed for serialization.
GenericValue(){}
/// Construct from value
GenericValue(const T& value) :
@ -131,31 +72,123 @@ public:
// Cast the base class Value pointer to a templated generic class pointer
const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
// Return the result of using the equals traits for the derived class
return traits::equals<T>()(this->value_, genericValue2.value_, tol);
return traits<T>::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<T>()(this->value(), other.value(), tol);
return traits<T>::Equals(this->value(), other.value(), tol);
}
/// Virtual print function, uses traits
virtual void print(const std::string& str) const {
traits::print<T>()(value_, str);
traits<T>::Print(value_, str);
}
// Serialization below:
/**
* 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<PoolTag, sizeof(GenericValue)>::malloc();
GenericValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in
return ptr;
}
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
ar & BOOST_SERIALIZATION_NVP(value_);
}
/**
* 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<PoolTag, sizeof(GenericValue)>::free((void*) this); // Release memory from pool
}
protected:
/**
* Clone this value (normal clone on the heap, delete with 'delete' operator)
*/
virtual boost::shared_ptr<Value> clone() const {
return boost::make_shared<GenericValue>(*this);
}
// Assignment operator for this class not needed since GenericValue<T> is an abstract class
/// 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<T>::Retract(GenericValue<T>::value(), delta);
// Create a Value pointer copy of the result
void* resultAsValuePlace =
boost::singleton_pool<PoolTag, sizeof(GenericValue)>::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<T>& genericValue2 =
static_cast<const GenericValue<T>&>(value2);
// Return the result of calling localCoordinates trait on the derived class
return traits<T>::Local(GenericValue<T>::value(), genericValue2.value());
}
/// Non-virtual version of retract
GenericValue retract(const Vector& delta) const {
return GenericValue(traits<T>::Retract(GenericValue<T>::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<T>::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<const GenericValue&>(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<DERIVED>& operator=(const DerivedValue<DERIVED>& 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<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("GenericValue",
boost::serialization::base_object<Value>(*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<Type>)
};

96
gtsam/base/Group.h
View File

@ -19,43 +19,93 @@
#pragma once
#include <gtsam/global_includes.h>
#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <boost/static_assert.hpp>
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 <typename T> 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 T>
class GroupConcept {
private:
static T concept_check(const T& t) {
/** assignment */
T t2 = t;
template<typename G>
class IsGroup {
public:
typedef typename traits<G>::structure_category structure_category_tag;
typedef typename traits<G>::group_flavor flavor_tag;
//typedef typename traits<G>::identity::value_type identity_value_type;
/** identity */
T identity = T::identity();
/** compose with another object */
T compose_ret = identity.compose(t2);
/** invert the object and yield a new one */
T inverse_ret = compose_ret.inverse();
return inverse_ret;
BOOST_CONCEPT_USAGE(IsGroup) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<group_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a group (or derived)");
e = traits<G>::Identity();
e = traits<G>::Compose(g, h);
e = traits<G>::Between(g, h);
e = traits<G>::Inverse(g);
operator_usage(flavor);
// todo: how do we test the act concept? or do we even need to?
}
private:
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;
}
flavor_tag flavor;
G e, g, h;
bool b;
};
} // \namespace gtsam
/// Check invariants
template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
G e = traits<G>::Identity();
return traits<G>::Equals(traits<G>::Compose(a, traits<G>::Inverse(a)), e, tol)
&& traits<G>::Equals(traits<G>::Between(a, b), traits<G>::Compose(traits<G>::Inverse(a), b), tol)
&& traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
}
/// 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;}
/// 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
/**
* 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<T>;
#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T;
#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup<T>;
#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup<T> _gtsam_IsGroup_##T;

297
gtsam/base/Lie.h
View File

@ -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,245 @@
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<Class,N>::inverse"
/// For derivative math, see doc/math.pdf
template <class Class, int N>
struct LieGroup {
BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic,
"LieGroup not yet specialized for dynamically sized types.");
enum { dimension = N };
typedef OptionalJacobian<N, N> ChartJacobian;
typedef Eigen::Matrix<double, N, N> Jacobian;
typedef Eigen::Matrix<double, N, 1> TangentVector;
const Class & derived() const {
return static_cast<const Class&>(*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<double, N, N>::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<double, N, N>::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,D_g_v);
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, D_v_h);
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<Class> : public internal::LieGroupTraits<Class> {};
template<class Class>
struct LieGroupTraits : Testable<Class> {
typedef lie_group_tag structure_category;
/// @name Group
/// @{
typedef multiplicative_group_tag group_flavor;
static Class Identity() { return Class::identity();}
static Class Compose(const Class& m1, const Class& m2) { return m1 * m2;}
static Class Between(const Class& m1, const Class& m2) { return m1.inverse() * m2;}
static Class Inverse(const Class& m) { return m.inverse();}
/// @}
/// @name Manifold
/// @{
typedef Class ManifoldType;
enum { dimension = Class::dimension };
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> 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) {
return origin.localCoordinates(other);
}
static Class Retract(const Class& origin, const TangentVector& v) {
return origin.retract(v);
}
static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian Horigin, ChartJacobian Hother = boost::none) {
return origin.localCoordinates(other, Horigin, Hother);
}
static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian Horigin, 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,
ChartJacobian H2 = boost::none) {
return m1.compose(m2, H1, H2);
}
static Class Between(const Class& m1, const Class& m2, ChartJacobian H1,
ChartJacobian H2 = boost::none) {
return m1.between(m2, H1, H2);
}
static Class Inverse(const Class& m, ChartJacobian H) {
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<class T>
inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);}
template<class Class>
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<class T>
inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));}
template<class Class>
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<class T>
inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));}
template<class Class>
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 T>
class LieConcept {
private:
/** concept checking function - implement the functions this demands */
static T concept_check(const T& t) {
template<typename T>
class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
public:
typedef typename traits<T>::structure_category structure_category_tag;
typedef typename traits<T>::ManifoldType ManifoldType;
typedef typename traits<T>::TangentVector TangentVector;
typedef typename traits<T>::ChartJacobian ChartJacobian;
/** assignment */
T t2 = t;
BOOST_CONCEPT_USAGE(IsLieGroup) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<lie_group_tag, structure_category_tag>::value),
"This type's trait does not assert it is a Lie group (or derived)");
/**
* Returns dimensionality of the tangent space
*/
size_t dim_ret = t.dim();
/** 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<T>::Compose(g, h, Hg, Hh);
g = traits<T>::Between(g, h, Hg, Hh);
g = traits<T>::Inverse(g, Hg);
// log and exp map without Jacobians
g = traits<T>::Expmap(v);
v = traits<T>::Logmap(g);
// log and expnential map with Jacobians
g = traits<T>::Expmap(v, Hg);
v = traits<T>::Logmap(g, Hg);
}
private:
T g, h;
TangentVector v;
ChartJacobian Hg, Hh;
};
/**
@ -146,12 +312,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<T>; \
template class gtsam::GroupConcept<T>; \
template class gtsam::LieConcept<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) \
typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T; \
typedef gtsam::GroupConcept<T> _gtsam_GroupConcept_##T; \
typedef gtsam::LieConcept<T> _gtsam_LieConcept_##T;
#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup<T> _gtsam_IsLieGroup_##T;

138
gtsam/base/LieMatrix.h
View File

@ -25,15 +25,13 @@
#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
#endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/VectorSpace.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
/**
* @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
@ -41,13 +39,17 @@ struct LieMatrix : public Matrix {
/// @name Constructors
/// @{
enum { dimension = Eigen::Dynamic };
/** default constructor - should be unnecessary */
/** default constructor - only for serialize */
LieMatrix() {}
/** initialize from a normal matrix */
LieMatrix(const Matrix& v) : Matrix(v) {}
template <class M>
LieMatrix(const M& v) : Matrix(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
@ -70,102 +72,57 @@ struct LieMatrix : public Matrix {
inline bool equals(const LieMatrix& expected, double tol=1e-5) const {
return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol);
}
/// @}
/// @name Standard Interface
/// @{
/** get the underlying vector */
/** get the underlying matrix */
inline Matrix matrix() const {
return static_cast<Matrix>(*this);
}
/// @}
/// @name Manifold interface
/// @name Group
/// @{
LieMatrix compose(const LieMatrix& q) { return (*this)+q;}
LieMatrix between(const LieMatrix& q) { return q-(*this);}
LieMatrix inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
Vector localCoordinates(const LieMatrix& q) { return between(q).vector();}
LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));}
/// @}
/// @name Lie Group
/// @{
static Vector Logmap(const LieMatrix& p) {return p.vector();}
static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);}
/// @}
/// @name VectorSpace requirements
/// @{
/** Returns dimensionality of the tangent space */
inline size_t dim() const { return this->size(); }
inline size_t dim() const { return size(); }
/** Update the LieMatrix with a tangent space update. The elements of the
* tangent space vector correspond to the matrix entries arranged in
* *row-major* order. */
inline LieMatrix retract(const Vector& v) const {
if(v.size() != this->size())
throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size");
return LieMatrix(*this +
Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
&v(0), this->rows(), this->cols()));
}
/** @return the local coordinates of another object. The elements of the
* tangent space vector correspond to the matrix entries arranged in
* *row-major* order. */
inline Vector localCoordinates(const LieMatrix& t2) const {
Vector result(this->size());
Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
&result(0), this->rows(), this->cols()) = t2 - *this;
/** Convert to vector, is done row-wise - TODO why? */
inline Vector vector() const {
Vector result(size());
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
Eigen::Map<RowMajor>(&result(0), rows(), cols()) = *this;
return result;
}
/// @}
/// @name Group interface
/// @{
/** identity - NOTE: no known size at compile time - so zero length */
inline static LieMatrix identity() {
throw std::runtime_error("LieMatrix::identity(): Don't use this function");
return LieMatrix();
}
// Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments
// This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class
// instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector
// as the other geometry objects (Point3, Rot3, etc.) have this problem
/** compose with another object */
inline LieMatrix compose(const LieMatrix& p,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) *H1 = eye(dim());
if(H2) *H2 = eye(p.dim());
return LieMatrix(*this + p);
}
/** between operation */
inline LieMatrix between(const LieMatrix& l2,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(dim());
if(H2) *H2 = eye(l2.dim());
return LieMatrix(l2 - *this);
}
/** invert the object and yield a new one */
inline LieMatrix inverse(boost::optional<gtsam::Matrix&> H=boost::none) const {
if(H) *H = -eye(dim());
return LieMatrix(-(*this));
}
/// @}
/// @name Lie group interface
/// @{
/** Expmap around identity */
static inline LieMatrix Expmap(const Vector& v) {
throw std::runtime_error("LieMatrix::Expmap(): Don't use this function");
return LieMatrix(v); }
/** Logmap around identity */
static inline Vector Logmap(const LieMatrix& p) {
Vector result(p.size());
Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> >(
result.data(), p.rows(), p.cols()) = p;
return result;
}
/// @}
private:
@ -181,17 +138,20 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<LieMatrix> : public boost::true_type {
};
struct traits<LieMatrix> : public internal::VectorSpace<LieMatrix> {
template<>
struct dimension<LieMatrix> : public Dynamic {
};
// Override Retract, as the default version does not know how to initialize
static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin);
typedef const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic,
Eigen::RowMajor> RowMajor;
return origin + Eigen::Map<RowMajor>(&v(0), origin.rows(), origin.cols());
}
}
};
} // \namespace gtsam

105
gtsam/base/LieScalar.h
View File

@ -24,8 +24,7 @@
#endif
#include <gtsam/dllexport.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/VectorSpace.h>
namespace gtsam {
@ -36,11 +35,13 @@ namespace gtsam {
*/
struct GTSAM_EXPORT LieScalar {
enum { dimension = 1 };
/** default constructor */
LieScalar() : d_(0.0) {}
/** wrap a double */
explicit LieScalar(double d) : d_(d) {}
/*explicit*/ LieScalar(double d) : d_(d) {}
/** access the underlying value */
double value() const { return d_; }
@ -48,93 +49,43 @@ namespace gtsam {
/** Automatic conversion to underlying value */
operator double() const { return d_; }
/** print @param name optional string naming the object */
void print(const std::string& name="") const;
/** convert vector */
Vector1 vector() const { Vector1 v; v<<d_; return v; }
/** equality up to tolerance */
/// @name Testable
/// @{
void print(const std::string& name="") const;
bool equals(const LieScalar& expected, double tol=1e-5) const {
return fabs(expected.d_ - d_) <= tol;
}
/// @}
// Manifold requirements
/// @name Group
/// @{
static LieScalar identity() { return LieScalar(0);}
LieScalar compose(const LieScalar& q) { return (*this)+q;}
LieScalar between(const LieScalar& q) { return q-(*this);}
LieScalar inverse() { return -(*this);}
/// @}
/** Returns dimensionality of the tangent space */
/// @name Manifold
/// @{
size_t dim() const { return 1; }
static size_t Dim() { return 1; }
Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();}
LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));}
/// @}
/** Update the LieScalar with a tangent space update */
LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); }
/** @return the local coordinates of another object */
Vector localCoordinates(const LieScalar& t2) const { return (Vector(1) << (t2.value() - value())).finished(); }
// Group requirements
/** identity */
static LieScalar identity() {
return LieScalar();
}
/** compose with another object */
LieScalar compose(const LieScalar& p,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = eye(1);
if(H2) *H2 = eye(1);
return LieScalar(d_ + p.d_);
}
/** between operation */
LieScalar between(const LieScalar& l2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(1);
if(H2) *H2 = eye(1);
return LieScalar(l2.value() - value());
}
/** invert the object and yield a new one */
LieScalar inverse() const {
return LieScalar(-1.0 * value());
}
// Lie functions
/** Expmap around identity */
static LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); }
/** Logmap around identity - just returns with default cast back */
static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); }
/// Left-trivialized derivative of the exponential map
static Matrix ExpmapDerivative(const Vector& v) {
return eye(1);
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix LogmapDerivative(const Vector& v) {
return eye(1);
}
/// @name Lie Group
/// @{
static Vector1 Logmap(const LieScalar& p) { return p.vector();}
static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);}
/// @}
private:
double d_;
};
// Define GTSAM traits
namespace traits {
template<>
struct is_group<LieScalar> : public boost::true_type {
};
template<>
struct is_manifold<LieScalar> : public boost::true_type {
};
template<>
struct dimension<LieScalar> : public boost::integral_constant<int, 1> {
};
}
struct traits<LieScalar> : public internal::ScalarTraits<LieScalar> {};
} // \namespace gtsam

2
gtsam/base/LieVector.cpp
View File

@ -35,4 +35,6 @@ void LieVector::print(const std::string& name) const {
gtsam::print(vector(), name);
}
// Does not compile because LieVector is not fixed size.
// GTSAM_CONCEPT_LIE_INST(LieVector)
} // \namespace gtsam

108
gtsam/base/LieVector.h
View File

@ -23,25 +23,28 @@
#warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
#endif
#include <gtsam/base/Lie.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/VectorSpace.h>
namespace gtsam {
/**
* @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
* we can directly add double, Vector, and Matrix into values now, because of
* gtsam::traits.
*/
struct LieVector : public Vector {
enum { dimension = Eigen::Dynamic };
/** default constructor - should be unnecessary */
LieVector() {}
/** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {}
template <class V>
LieVector(const V& v) : Vector(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */
@ -55,75 +58,51 @@ struct LieVector : public Vector {
/** constructor with size and initial data, row order ! */
GTSAM_EXPORT LieVector(size_t m, const double* const data);
/// @name Testable
/// @{
GTSAM_EXPORT void print(const std::string& name="") const;
bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol);
}
/// @}
/// @name Group
/// @{
LieVector compose(const LieVector& q) { return (*this)+q;}
LieVector between(const LieVector& q) { return q-(*this);}
LieVector inverse() { return -(*this);}
/// @}
/// @name Manifold
/// @{
Vector localCoordinates(const LieVector& q) { return between(q).vector();}
LieVector retract(const Vector& v) {return compose(LieVector(v));}
/// @}
/// @name Lie Group
/// @{
static Vector Logmap(const LieVector& p) {return p.vector();}
static LieVector Expmap(const Vector& v) { return LieVector(v);}
/// @}
/// @name VectorSpace requirements
/// @{
/** get the underlying vector */
Vector vector() const {
return static_cast<Vector>(*this);
}
/** print @param name optional string naming the object */
GTSAM_EXPORT void print(const std::string& name="") const;
/** equality up to tolerance */
bool equals(const LieVector& expected, double tol=1e-5) const {
return gtsam::equal(vector(), expected.vector(), tol);
}
// Manifold requirements
/** Returns dimensionality of the tangent space */
size_t dim() const { return this->size(); }
/** Update the LieVector with a tangent space update */
LieVector retract(const Vector& v) const { return LieVector(vector() + v); }
/** @return the local coordinates of another object */
Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); }
// Group requirements
/** identity - NOTE: no known size at compile time - so zero length */
static LieVector identity() {
throw std::runtime_error("LieVector::identity(): Don't use this function");
return LieVector();
}
// Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments
// This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class
// instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector
// as the other geometry objects (Point3, Rot3, etc.) have this problem
/** compose with another object */
LieVector compose(const LieVector& p,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) *H1 = eye(dim());
if(H2) *H2 = eye(p.dim());
return LieVector(vector() + p);
}
/** between operation */
LieVector between(const LieVector& l2,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(dim());
if(H2) *H2 = eye(l2.dim());
return LieVector(l2.vector() - vector());
}
/** invert the object and yield a new one */
LieVector inverse(boost::optional<gtsam::Matrix&> H=boost::none) const {
if(H) *H = -eye(dim());
return LieVector(-1.0 * vector());
}
// Lie functions
/** Expmap around identity */
static LieVector Expmap(const Vector& v) { return LieVector(v); }
/** Logmap around identity - just returns with default cast back */
static Vector Logmap(const LieVector& p) { return p; }
/// @}
private:
@ -136,17 +115,8 @@ private:
}
};
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<LieVector> : public boost::true_type {
};
template<>
struct dimension<LieVector> : public Dynamic {
};
}
struct traits<LieVector> : public internal::VectorSpace<LieVector> {};
} // \namespace gtsam

392
gtsam/base/Manifold.h
View File

@ -14,17 +14,24 @@
* @brief Base class and basic functions for Manifold types
* @author Alex Cunningham
* @author Frank Dellaert
* @author Mike Bosse
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <boost/static_assert.hpp>
#include <boost/type_traits.hpp>
#include <string>
#include <gtsam/base/Testable.h>
#include <gtsam/base/OptionalJacobian.h>
#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
namespace gtsam {
/// tag to assert a type is a manifold
struct manifold_tag {};
/**
* A manifold defines a space in which there is a notion of a linear tangent space
* that can be centered around a given point on the manifold. These nonlinear
@ -43,325 +50,130 @@ namespace gtsam {
*
*/
// Traits, same style as Boost.TypeTraits
// All meta-functions below ever only declare a single type
// or a type/value/value_type
namespace traits {
template <typename T> struct traits;
// is group, by default this is false
template<typename T>
struct is_group: public boost::false_type {
};
namespace internal {
// identity, no default provided, by default given by default constructor
template<typename T>
struct identity {
static T value() {
return T();
/// Requirements on type to pass it to Manifold template below
template<class Class>
struct HasManifoldPrereqs {
enum { dim = Class::dimension };
Class p, q;
Eigen::Matrix<double, dim, 1> v;
OptionalJacobian<dim, dim> Hp, Hq, Hv;
BOOST_CONCEPT_USAGE(HasManifoldPrereqs) {
v = p.localCoordinates(q);
q = p.retract(v);
}
};
// is manifold, by default this is false
template<typename T>
struct is_manifold: public boost::false_type {
};
// dimension, can return Eigen::Dynamic (-1) if not known at compile time
// defaults to dynamic, TODO makes sense ?
typedef boost::integral_constant<int, Eigen::Dynamic> Dynamic;
template<typename T>
struct dimension: public Dynamic {
};
/**
* zero<T>::value is intended to be the origin of a canonical coordinate system
* with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
* Below we provide the group identity as zero *in case* it is a group
*/
template<typename T> struct zero: public identity<T> {
BOOST_STATIC_ASSERT(is_group<T>::value);
};
// double
template<>
struct is_group<double> : public boost::true_type {
};
template<>
struct is_manifold<double> : public boost::true_type {
};
template<>
struct dimension<double> : public boost::integral_constant<int, 1> {
};
template<>
struct zero<double> {
static double value() {
return 0;
/// Extra manifold traits for fixed-dimension types
template<class Class, size_t N>
struct ManifoldImpl {
// Compile-time dimensionality
static int GetDimension(const Class&) {
return N;
}
};
// Fixed size Eigen::Matrix type
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct is_group<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
};
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
};
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<
int,
M == Eigen::Dynamic ? Eigen::Dynamic :
(N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
//TODO after switch to c++11 : the above should should be extracted to a constexpr function
// for readability and to reduce code duplication
};
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
"traits::zero is only supported for fixed-size matrices");
static Eigen::Matrix<double, M, N, Options> value() {
return Eigen::Matrix<double, M, N, Options>::Zero();
/// Extra manifold traits for variable-dimension types
template<class Class>
struct ManifoldImpl<Class, Eigen::Dynamic> {
// Run-time dimensionality
static int GetDimension(const Class& m) {
return m.dim();
}
};
template<int M, int N, int Options>
struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<
Eigen::Matrix<double, M, N, Options> > {
};
/// A helper that implements the traits interface for GTSAM manifolds.
/// To use this for your class type, define:
/// template<> struct traits<Class> : public Manifold<Class> { };
template<class Class>
struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
template<typename T> struct is_chart: public boost::false_type {
};
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
} // \ namespace traits
// Dimension of the manifold
enum { dimension = Class::dimension };
// Chart is a map from T -> vector, retract is its inverse
template<typename T>
struct DefaultChart {
//BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
typedef T type;
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
// Typedefs required by all manifold types.
typedef Class ManifoldType;
typedef manifold_tag structure_category;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
static vector local(const T& origin, const T& other) {
// Local coordinates
static TangentVector Local(const Class& origin, const Class& other) {
return origin.localCoordinates(other);
}
static T retract(const T& origin, const vector& d) {
return origin.retract(d);
}
static int getDimension(const T& origin) {
return origin.dim();
// Retraction back to manifold
static Class Retract(const Class& origin, const TangentVector& v) {
return origin.retract(v);
}
};
namespace traits {
// populate default traits
template<typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {
};
template<typename T> struct dimension<DefaultChart<T> > : public dimension<T> {
};
} // \ namespace internal
/// Check invariants for Manifold type
template<typename T>
BOOST_CONCEPT_REQUIRES(((IsTestable<T>)),(bool)) //
check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
typename traits<T>::TangentVector v0 = traits<T>::Local(a,a);
typename traits<T>::TangentVector v = traits<T>::Local(a,b);
T c = traits<T>::Retract(a,v);
return v0.norm() < tol && traits<T>::Equals(b,c,tol);
}
template<class C>
struct ChartConcept {
/// Manifold concept
template<typename T>
class IsManifold {
public:
typedef typename C::type type;
typedef typename C::vector vector;
BOOST_CONCEPT_USAGE(ChartConcept) {
// is_chart trait should be true
BOOST_STATIC_ASSERT((traits::is_chart<C>::value));
typedef typename traits<T>::structure_category structure_category_tag;
static const size_t dim = traits<T>::dimension;
typedef typename traits<T>::ManifoldType ManifoldType;
typedef typename traits<T>::TangentVector TangentVector;
/**
* Returns Retraction update of val_
*/
type retract_ret = C::retract(val_, vec_);
BOOST_CONCEPT_USAGE(IsManifold) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<manifold_tag, structure_category_tag>::value),
"This type's structure_category trait does not assert it as a manifold (or derived)");
BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
/**
* Returns local coordinates of another object
*/
vec_ = C::local(val_, retract_ret);
// a way to get the dimension that is compatible with dynamically sized types
dim_ = C::getDimension(val_);
// make sure Chart methods are defined
v = traits<T>::Local(p, q);
q = traits<T>::Retract(p, v);
}
private:
type val_;
vector vec_;
int dim_;
TangentVector v;
ManifoldType p, q;
};
/**
* CanonicalChart<Chart<T> > is a chart around zero<T>::value
* Canonical<T> is CanonicalChart<DefaultChart<T> >
* An example is Canonical<Rot3>
*/
template<typename C> struct CanonicalChart {
BOOST_CONCEPT_ASSERT((ChartConcept<C>));
typedef C Chart;
typedef typename Chart::type type;
typedef typename Chart::vector vector;
// Convert t of type T into canonical coordinates
vector local(const type& t) {
return Chart::local(traits::zero<type>::value(), t);
}
// Convert back from canonical coordinates to T
type retract(const vector& v) {
return Chart::retract(traits::zero<type>::value(), v);
}
};
template<typename T> struct Canonical: public CanonicalChart<DefaultChart<T> > {
};
// double
template<>
struct DefaultChart<double> {
typedef double type;
typedef Eigen::Matrix<double, 1, 1> vector;
static vector local(double origin, double other) {
vector d;
d << other - origin;
return d;
}
static double retract(double origin, const vector& d) {
return origin + d[0];
}
static int getDimension(double) {
return 1;
}
};
// Fixed size Eigen::Matrix type
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
/**
* This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options).
* Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size.
*/
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type;
typedef type T;
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
"Internal error: DefaultChart for Dynamic should be handled by template below");
static vector local(const T& origin, const T& other) {
return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other)
- reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
}
static T retract(const T& origin, const vector& d) {
return origin + reshape<M, N, Options>(d);
}
static int getDimension(const T&origin) {
return origin.rows() * origin.cols();
}
};
// Dynamically sized Vector
template<>
struct DefaultChart<Vector> {
typedef Vector type;
typedef Vector vector;
static vector local(const Vector& origin, const Vector& other) {
return other - origin;
}
static Vector retract(const Vector& origin, const vector& d) {
return origin + d;
}
static int getDimension(const Vector& origin) {
return origin.size();
}
};
// Dynamically sized Matrix
template<>
struct DefaultChart<Matrix> {
typedef Matrix type;
typedef Vector vector;
static vector local(const Matrix& origin, const Matrix& other) {
Matrix d = other - origin;
return Eigen::Map<Vector>(d.data(),getDimension(d));
}
static Matrix retract(const Matrix& origin, const vector& d) {
return origin + Eigen::Map<const Matrix>(d.data(),origin.rows(),origin.cols());
}
static int getDimension(const Matrix& m) {
return m.size();
}
};
/**
* Old Concept check class for Manifold types
* Requires a mapping between a linear tangent space and the underlying
* manifold, of which Lie is a specialization.
*
* The necessary functions to implement for Manifold are defined
* below with additional details as to the interface. The
* concept checking function in class Manifold will check whether or not
* the function exists and throw compile-time errors.
*
* Returns dimensionality of the tangent space, which may be smaller than the number
* of nonlinear parameters.
* size_t dim() const;
*
* Returns a new T that is a result of updating *this with the delta v after pulling
* the updated value back to the manifold T.
* T retract(const Vector& v) const;
*
* Returns the linear coordinates of lp in the tangent space centered around *this.
* Vector localCoordinates(const T& lp) const;
*
* By convention, we use capital letters to designate a static function
* @tparam T is a Lie type, like Point2, Pose3, etc.
*/
template<class T>
class ManifoldConcept {
private:
/** concept checking function - implement the functions this demands */
static T concept_check(const T& t) {
/** assignment */
T t2 = t;
/**
* Returns dimensionality of the tangent space
*/
size_t dim_ret = t.dim();
/**
* Returns Retraction update of T
*/
T retract_ret = t.retract(gtsam::zero(dim_ret));
/**
* Returns local coordinates of another object
*/
Vector localCoords_ret = t.localCoordinates(t2);
return retract_ret;
}
/// Give fixed size dimension of a type, fails at compile time if dynamic
template<typename T>
struct FixedDimension {
typedef const int value_type;
static const int value = traits<T>::dimension;
BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
"FixedDimension instantiated for dymanically-sized type.");
};
} // \ namespace gtsam
/**
* Macros for using the ManifoldConcept
* - 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_MANIFOLD_INST(T) template class gtsam::ManifoldConcept<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T;
///**
// * Macros for using the ManifoldConcept
// * - 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_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold<T> _gtsam_IsManifold_##T;

18
gtsam/base/Matrix.cpp
View File

@ -238,11 +238,6 @@ istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
return inputStream;
}
/* ************************************************************************* */
void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) {
fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
}
/* ************************************************************************* */
Matrix diag(const std::vector<Matrix>& Hs) {
size_t rows = 0, cols = 0;
@ -665,19 +660,6 @@ Matrix expm(const Matrix& A, size_t K) {
return E;
}
/* ************************************************************************* */
Matrix Cayley(const Matrix& A) {
Matrix::Index n = A.cols();
assert(A.rows() == n);
// original
// const Matrix I = eye(n);
// return (I-A)*inverse(I+A);
// inlined to let Eigen do more optimization
return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse();
}
/* ************************************************************************* */
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal)
{

19
gtsam/base/Matrix.h
View File

@ -237,7 +237,10 @@ Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1,
* @param i is the row of the upper left corner insert location
* @param j is the column of the upper left corner insert location
*/
GTSAM_EXPORT void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j);
template <typename Derived1, typename Derived2>
GTSAM_EXPORT void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
}
/**
* Create a matrix with submatrices along its diagonal
@ -525,17 +528,6 @@ DLT(const Matrix& A, double rank_tol = 1e-9);
*/
GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);
/// Cayley transform
GTSAM_EXPORT Matrix Cayley(const Matrix& A);
/// Implementation of Cayley transform using fixed size matrices to let
/// Eigen do more optimization
template<int N>
Eigen::Matrix<double, N, N> CayleyFixed(const Eigen::Matrix<double, N, N>& A) {
typedef Eigen::Matrix<double, N, N> FMat;
return (FMat::Identity() - A)*(FMat::Identity() + A).inverse();
}
std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
} // namespace gtsam
@ -568,4 +560,5 @@ namespace boost {
} // namespace serialization
} // namespace boost
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix)
BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix);

75
gtsam/base/OptionalJacobian.h
View File

@ -11,7 +11,7 @@
/**
* @file OptionalJacobian.h
* @brief Special class for optional Matrix arguments
* @brief Special class for optional Jacobian arguments
* @author Frank Dellaert
* @author Natesh Srinivasan
* @date Nov 28, 2014
@ -33,23 +33,24 @@ namespace gtsam {
* matrix will be resized. Finally, there is a constructor that takes
* boost::none, the default constructor acts like boost::none, and
* boost::optional<Eigen::MatrixXd&> is also supported for backwards compatibility.
* Below this class, a dynamic version is also implemented.
*/
template<int Rows, int Cols>
class OptionalJacobian {
public:
/// Fixed size type
typedef Eigen::Matrix<double, Rows, Cols> Fixed;
/// ::Jacobian size type
typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
private:
Eigen::Map<Fixed> map_; /// View on constructor argument, if given
Eigen::Map<Jacobian> map_; /// View on constructor argument, if given
// Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
// uses "placement new" to make map_ usurp the memory of the fixed size matrix
void usurp(double* data) {
new (&map_) Eigen::Map<Fixed>(data);
new (&map_) Eigen::Map<Jacobian>(data);
}
public:
@ -60,13 +61,13 @@ public:
}
/// Constructor that will usurp data of a fixed-size matrix
OptionalJacobian(Fixed& fixed) :
OptionalJacobian(Jacobian& fixed) :
map_(NULL) {
usurp(fixed.data());
}
/// Constructor that will usurp data of a fixed-size matrix, pointer version
OptionalJacobian(Fixed* fixedPtr) :
OptionalJacobian(Jacobian* fixedPtr) :
map_(NULL) {
if (fixedPtr)
usurp(fixedPtr->data());
@ -103,12 +104,68 @@ public:
}
/// De-reference, like boost optional
Eigen::Map<Fixed>& operator*() {
Eigen::Map<Jacobian>& operator*() {
return map_;
}
/// TODO: operator->()
Eigen::Map<Fixed>* operator->(){ return &map_; }
Eigen::Map<Jacobian>* operator->(){ return &map_; }
};
// The pure dynamic specialization of this is needed to support
// variable-sized types. Note that this is designed to work like the
// boost optional scheme from GTSAM 3.
template<>
class OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> {
public:
/// Jacobian size type
typedef Eigen::MatrixXd Jacobian;
private:
Jacobian* pointer_; /// View on constructor argument, if given
public:
/// Default constructor acts like boost::none
OptionalJacobian() :
pointer_(NULL) {
}
/// Constructor that will resize a dynamic matrix (unless already correct)
OptionalJacobian(Eigen::MatrixXd& dynamic) :
pointer_(&dynamic) {
}
#ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t none) :
pointer_(NULL) {
}
/// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
pointer_(NULL) {
if (optional) pointer_ = &(*optional);
}
#endif
/// Return true is allocated, false if default constructor was used
operator bool() const {
return pointer_;
}
/// De-reference, like boost optional
Jacobian& operator*() {
return *pointer_;
}
/// TODO: operator->()
Jacobian* operator->(){ return pointer_; }
};
} // namespace gtsam

87
gtsam/base/Testable.h
View File

@ -34,6 +34,7 @@
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/concept_check.hpp>
#include <stdio.h>
#include <string>
@ -41,45 +42,52 @@
namespace gtsam {
// Forward declaration
template <typename T> struct traits;
/**
* A testable concept check that should be placed in applicable unit
* tests and in generic algorithms.
*
* See macros for details on using this structure
* @addtogroup base
* @tparam T is the type this constrains to be testable - assumes print() and equals()
* @tparam T is the objectype this constrains to be testable - assumes print() and equals()
*/
template <class T>
class TestableConcept {
static bool checkTestableConcept(const T& d) {
class IsTestable {
T t;
bool r1,r2;
public:
BOOST_CONCEPT_USAGE(IsTestable) {
// check print function, with optional string
d.print(std::string());
d.print();
traits<T>::Print(t, std::string());
traits<T>::Print(t);
// check print, with optional threshold
double tol = 1.0;
bool r1 = d.equals(d, tol);
bool r2 = d.equals(d);
return r1 && r2;
r1 = traits<T>::Equals(t,t,tol);
r2 = traits<T>::Equals(t,t);
}
};
}; // \ Testable
/** Call print on the object */
template<class T>
inline void print(const T& object, const std::string& s = "") {
object.print(s);
inline void print(float v, const std::string& s = "") {
printf("%s%f\n",s.c_str(),v);
}
inline void print(double v, const std::string& s = "") {
printf("%s%lf\n",s.c_str(),v);
}
/** Call equal on the object */
template<class T>
inline bool equal(const T& obj1, const T& obj2, double tol) {
return obj1.equals(obj2, tol);
return traits<T>::Equals(obj1,obj2, tol);
}
/** Call equal on the object without tolerance (use default tolerance) */
/** Call equal without tolerance (use default tolerance) */
template<class T>
inline bool equal(const T& obj1, const T& obj2) {
return obj1.equals(obj2);
return traits<T>::Equals(obj1,obj2);
}
/**
@ -87,11 +95,11 @@ namespace gtsam {
*/
template<class V>
bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) {
if (actual.equals(expected, tol))
if (traits<V>::Equals(actual,expected, tol))
return true;
printf("Not equal:\n");
expected.print("expected:\n");
actual.print("actual:\n");
traits<V>::Print(expected,"expected:\n");
traits<V>::Print(actual,"actual:\n");
return false;
}
@ -103,7 +111,7 @@ namespace gtsam {
double tol_;
equals(double tol = 1e-9) : tol_(tol) {}
bool operator()(const V& expected, const V& actual) {
return (actual.equals(expected, tol_));
return (traits<V>::Equals(actual, expected, tol_));
}
};
@ -116,7 +124,39 @@ namespace gtsam {
equals_star(double tol = 1e-9) : tol_(tol) {}
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
if (!actual || !expected) return false;
return (actual->equals(*expected, tol_));
return (traits<V>::Equals(*actual,*expected, tol_));
}
};
/// Requirements on type to pass it to Testable template below
template<typename T>
struct HasTestablePrereqs {
BOOST_CONCEPT_USAGE(HasTestablePrereqs) {
t->print(str);
b = t->equals(*s,tol);
}
T *t, *s; // Pointer is to allow abstract classes
bool b;
double tol;
std::string str;
};
/// A helper that implements the traits interface for GTSAM types.
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public Testable<Type> { };
template<typename T>
struct Testable {
// Check that T has the necessary methods
BOOST_CONCEPT_ASSERT((HasTestablePrereqs<T>));
static void Print(const T& m, const std::string& str = "") {
m.print(str);
}
static bool Equals(const T& m1, const T& m2, double tol = 1e-8) {
return m1.equals(m2, tol);
}
};
@ -129,6 +169,7 @@ namespace gtsam {
*
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable
* @deprecated please use BOOST_CONCEPT_ASSERT and
*/
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept<T>;
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept<T> _gtsam_TestableConcept_##T;
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>;
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::IsTestable<T> _gtsam_Testable_##T;

3
gtsam/base/Value.h
View File

@ -168,7 +168,8 @@ namespace gtsam {
* */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {}
void serialize(ARCHIVE & ar, const unsigned int version) {
}
};

471
gtsam/base/VectorSpace.h Normal file
View File

@ -0,0 +1,471 @@
/*
* VectorSpace.h
*
* @date December 21, 2014
* @author Mike Bosse
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/base/Lie.h>
namespace gtsam {
/// tag to assert a type is a vector space
struct vector_space_tag: public lie_group_tag {
};
template<typename T> struct traits;
namespace internal {
/// VectorSpace Implementation for Fixed sizes
template<class Class, int N>
struct VectorSpaceImpl {
/// @name Group
/// @{
static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
static Class Inverse(const Class& m) { return -m;}
/// @}
/// @name Manifold
/// @{
typedef Eigen::Matrix<double, N, 1> TangentVector;
typedef OptionalJacobian<N, N> ChartJacobian;
typedef Eigen::Matrix<double, N, N> Jacobian;
static int GetDimension(const Class&) { return N;}
static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = - Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
Class v = other-origin;
return v.vector();
}
static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
return origin + (Class)v;
}
/// @}
/// @name Lie Group
/// @{
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
if (Hm) *Hm = Jacobian::Identity();
return m.vector();
}
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
if (Hv) *Hv = Jacobian::Identity();
return Class(v);
}
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2) {
if (H1) *H1 = Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
return v1 + v2;
}
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2) {
if (H1) *H1 = - Jacobian::Identity();
if (H2) *H2 = Jacobian::Identity();
return v2 - v1;
}
static Class Inverse(const Class& v, ChartJacobian H) {
if (H) *H = - Jacobian::Identity();
return -v;
}
/// @}
};
/// VectorSpace implementation for dynamic types.
template<class Class>
struct VectorSpaceImpl<Class,Eigen::Dynamic> {
/// @name Group
/// @{
static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
static Class Inverse(const Class& m) { return -m;}
/// @}
/// @name Manifold
/// @{
typedef Eigen::VectorXd TangentVector;
typedef OptionalJacobian<Eigen::Dynamic,Eigen::Dynamic> ChartJacobian;
static int GetDimension(const Class& m) { return m.dim();}
static Eigen::MatrixXd Eye(const Class& m) {
int dim = GetDimension(m);
return Eigen::MatrixXd::Identity(dim, dim);
}
static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = - Eye(origin);
if (H2) *H2 = Eye(other);
Class v = other-origin;
return v.vector();
}
static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(origin);
if (H2) *H2 = Eye(origin);
return origin + Class(v);
}
/// @}
/// @name Lie Group
/// @{
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
if (Hm) *Hm = Eye(m);
return m.vector();
}
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
Class result(v);
if (Hv)
*Hv = Eye(v);
return result;
}
static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2) {
if (H1) *H1 = Eye(v1);
if (H2) *H2 = Eye(v2);
return v1 + v2;
}
static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
ChartJacobian H2) {
if (H1) *H1 = - Eye(v1);
if (H2) *H2 = Eye(v2);
return v2 - v1;
}
static Class Inverse(const Class& v, ChartJacobian H) {
if (H) *H = -Eye(v);
return -v;
}
/// @}
};
/// A helper that implements the traits interface for GTSAM lie groups.
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public VectorSpace<Type> { };
template<class Class>
struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
typedef vector_space_tag structure_category;
/// @name Group
/// @{
typedef additive_group_tag group_flavor;
static Class Identity() { return Class::identity();}
/// @}
/// @name Manifold
/// @{
enum { dimension = Class::dimension};
typedef Class ManifoldType;
/// @}
};
/// A helper that implements the traits interface for GTSAM lie groups.
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public ScalarTraits<Type> { };
template<typename Scalar>
struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
typedef vector_space_tag structure_category;
/// @name Testable
/// @{
static void Print(Scalar m, const std::string& str = "") {
gtsam::print(m, str);
}
static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) {
return std::abs(v1 - v2) < tol;
}
/// @}
/// @name Group
/// @{
typedef additive_group_tag group_flavor;
static Scalar Identity() { return 0;}
/// @}
/// @name Manifold
/// @{
typedef Scalar ManifoldType;
enum { dimension = 1 };
typedef Eigen::Matrix<double, 1, 1> TangentVector;
typedef OptionalJacobian<1, 1> ChartJacobian;
static TangentVector Local(Scalar origin, Scalar other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1)[0] = -1.0;
if (H2) (*H2)[0] = 1.0;
TangentVector result;
result(0) = other - origin;
return result;
}
static Scalar Retract(Scalar origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1)[0] = 1.0;
if (H2) (*H2)[0] = 1.0;
return origin + v[0];
}
/// @}
/// @name Lie Group
/// @{
static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) {
if (H) (*H)[0] = 1.0;
return Local(0, m);
}
static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
if (H) (*H)[0] = 1.0;
return v[0];
}
/// @}
};
} // namespace internal
/// double
template<> struct traits<double> : public internal::ScalarTraits<double> {
};
/// float
template<> struct traits<float> : public internal::ScalarTraits<float> {
};
// traits for any fixed double Eigen matrix
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
internal::VectorSpaceImpl<
Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols>, M * N> {
typedef vector_space_tag structure_category;
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Fixed;
/// @name Testable
/// @{
static void Print(const Fixed& m, const std::string& str = "") {
gtsam::print(Eigen::MatrixXd(m), str);
}
static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) {
return equal_with_abs_tol(v1, v2, tol);
}
/// @}
/// @name Group
/// @{
typedef additive_group_tag group_flavor;
static Fixed Identity() { return Fixed::Zero();}
/// @}
/// @name Manifold
/// @{
enum { dimension = M*N};
typedef Fixed ManifoldType;
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static TangentVector Local(Fixed origin, Fixed other,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1) = -Jacobian::Identity();
if (H2) (*H2) = Jacobian::Identity();
TangentVector result;
Eigen::Map<Fixed>(result.data()) = other - origin;
return result;
}
static Fixed Retract(Fixed origin, const TangentVector& v,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) (*H1) = Jacobian::Identity();
if (H2) (*H2) = Jacobian::Identity();
return origin + Eigen::Map<const Fixed>(v.data());
}
/// @}
/// @name Lie Group
/// @{
static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) {
if (H) *H = Jacobian::Identity();
TangentVector result;
Eigen::Map<Fixed>(result.data()) = m;
return result;
}
static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
Fixed m;
m.setZero();
if (H) *H = Jacobian::Identity();
return m + Eigen::Map<const Fixed>(v.data());
}
/// @}
};
namespace internal {
// traits for dynamic Eigen matrices
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct DynamicTraits {
typedef vector_space_tag structure_category;
typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Dynamic;
/// @name Testable
/// @{
static void Print(const Dynamic& m, const std::string& str = "") {
gtsam::print(Eigen::MatrixXd(m), str);
}
static bool Equals(const Dynamic& v1, const Dynamic& v2,
double tol = 1e-8) {
return equal_with_abs_tol(v1, v2, tol);
}
/// @}
/// @name Group
/// @{
typedef additive_group_tag group_flavor;
static Dynamic Identity() {
throw std::runtime_error("Identity not defined for dynamic types");
}
/// @}
/// @name Manifold
/// @{
enum { dimension = Eigen::Dynamic };
typedef Eigen::VectorXd TangentVector;
typedef Eigen::MatrixXd Jacobian;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
typedef Dynamic ManifoldType;
static int GetDimension(const Dynamic& m) {
return m.rows() * m.cols();
}
static Jacobian Eye(const Dynamic& m) {
int dim = GetDimension(m);
return Eigen::Matrix<double, dimension, dimension>::Identity(dim, dim);
}
static TangentVector Local(const Dynamic& m, const Dynamic& other, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = -Eye(m);
if (H2) *H2 = Eye(m);
TangentVector v(GetDimension(m));
Eigen::Map<Dynamic>(v.data(), m.rows(), m.cols()) = other - m;
return v;
}
static Dynamic Retract(const Dynamic& m, const TangentVector& v, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(m);
if (H2) *H2 = Eye(m);
return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols());
}
/// @}
/// @name Lie Group
/// @{
static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) {
if (H) *H = Eye(m);
TangentVector result(GetDimension(m));
Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
return result;
}
static Dynamic Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
throw std::runtime_error("Expmap not defined for dynamic types");
}
static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) {
if (H) *H = -Eye(m);
return -m;
}
static Dynamic Compose(const Dynamic& v1, const Dynamic& v2,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = Eye(v1);
if (H2) *H2 = Eye(v1);
return v1 + v2;
}
static Dynamic Between(const Dynamic& v1, const Dynamic& v2,
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
if (H1) *H1 = -Eye(v1);
if (H2) *H2 = Eye(v1);
return v2 - v1;
}
/// @}
};
} // \ internal
// traits for fully dynamic matrix
template<int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<double, -1, -1, Options, MaxRows, MaxCols> > :
public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> {
};
// traits for dynamic column vector
template<int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<double, -1, 1, Options, MaxRows, MaxCols> > :
public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> {
};
// traits for dynamic row vector
template<int Options, int MaxRows, int MaxCols>
struct traits<Eigen::Matrix<double, 1, -1, Options, MaxRows, MaxCols> > :
public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> {
};
/// Vector Space concept
template<typename T>
class IsVectorSpace: public IsLieGroup<T> {
public:
typedef typename traits<T>::structure_category structure_category_tag;
BOOST_CONCEPT_USAGE(IsVectorSpace) {
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<vector_space_tag, structure_category_tag>::value),
"This type's trait does not assert it as a vector space (or derived)");
r = p + q;
r = -p;
r = p - q;
}
private:
T p, q, r;
};
} // namespace gtsam

3
gtsam/base/chartTesting.h
View File

@ -31,6 +31,9 @@ template<typename T>
void testDefaultChart(TestResult& result_,
const std::string& name_,
const T& value) {
GTSAM_CONCEPT_TESTABLE_TYPE(T);
typedef typename gtsam::DefaultChart<T> Chart;
typedef typename Chart::vector Vector;

32
gtsam/base/concepts.h Normal file
View File

@ -0,0 +1,32 @@
/*
* concepts.h
*
* @date Dec 4, 2014
* @author Mike Bosse
* @author Frank Dellaert
*/
#pragma once
// This is a helper to ease the transition to the new traits defined in this file.
// Uncomment this if you want all methods that are tagged as not implemented
// to cause compilation errors.
#ifdef COMPILE_ERROR_NOT_IMPLEMENTED
#include <boost/static_assert.hpp>
#define CONCEPT_NOT_IMPLEMENTED BOOST_STATIC_ASSERT_MSG(boost::false_type, \
"This method is required by the new concepts framework but has not been implemented yet.");
#else
#include <exception>
#define CONCEPT_NOT_IMPLEMENTED \
throw std::runtime_error("This method is required by the new concepts framework but has not been implemented yet.");
#endif
namespace gtsam {
template <typename T> struct traits;
}

164
gtsam/base/numericalDerivative.h
View File

@ -28,11 +28,10 @@
#pragma GCC diagnostic pop
#endif
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/base/Lie.h>
namespace gtsam {
@ -58,25 +57,32 @@ namespace gtsam {
* http://www.boost.org/doc/libs/release/libs/bind/bind.html
*/
// a quick helper struct to get the appropriate fixed sized matrix from two value types
namespace internal {
template<class Y, class X=double>
struct FixedSizeMatrix {
typedef Eigen::Matrix<double,traits<Y>::dimension, traits<X>::dimension> type;
};
}
/**
* Numerically compute gradient of scalar function
* Class X is the input argument
* The class X needs to have dim, expmap, logmap
*/
template<class X>
Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
typename internal::FixedSizeMatrix<X>::type numericalGradient(boost::function<double(const X&)> h, const X& x,
double delta = 1e-5) {
double factor = 1.0 / (2.0 * delta);
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
BOOST_STATIC_ASSERT_MSG(
(boost::is_base_of<manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
static const int N = traits::dimension<X>::value;
static const int N = traits<X>::dimension;
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
typedef DefaultChart<X> ChartX;
typedef typename ChartX::vector TangentX;
// get chart at x
ChartX chartX;
typedef typename traits<X>::TangentVector TangentX;
// Prepare a tangent vector to perturb x with, only works for fixed size
TangentX d;
@ -85,9 +91,9 @@ Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
Vector g = zero(N); // Can be fixed size
for (int j = 0; j < N; j++) {
d(j) = delta;
double hxplus = h(chartX.retract(x, d));
double hxplus = h(traits<X>::Retract(x, d));
d(j) = -delta;
double hxmin = h(chartX.retract(x, d));
double hxmin = h(traits<X>::Retract(x, d));
d(j) = 0;
g(j) = (hxplus - hxmin) * factor;
}
@ -104,35 +110,33 @@ Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
* Class X is the input argument
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X>
// TODO Should compute fixed-size matrix
Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
double delta = 1e-5) {
using namespace traits;
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
typedef typename internal::FixedSizeMatrix<Y,X>::type Matrix;
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
typedef DefaultChart<Y> ChartY;
typedef typename ChartY::vector TangentY;
typedef traits<Y> TraitsY;
typedef typename TraitsY::TangentVector TangentY;
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
static const int N = traits::dimension<X>::value;
static const int N = traits<X>::dimension;
BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
typedef DefaultChart<X> ChartX;
typedef typename ChartX::vector TangentX;
typedef traits<X> TraitsX;
typedef typename TraitsX::TangentVector TangentX;
// get value at x, and corresponding chart
Y hx = h(x);
ChartY chartY;
// Bit of a hack for now to find number of rows
TangentY zeroY = chartY.local(hx, hx);
TangentY zeroY = TraitsY::Local(hx, hx);
size_t m = zeroY.size();
// get chart at x
ChartX chartX;
// Prepare a tangent vector to perturb x with, only works for fixed size
TangentX dx;
dx.setZero();
@ -142,9 +146,9 @@ Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
double factor = 1.0 / (2.0 * delta);
for (int j = 0; j < N; j++) {
dx(j) = delta;
TangentY dy1 = chartY.local(hx, h(chartX.retract(x, dx)));
TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
dx(j) = -delta;
TangentY dy2 = chartY.local(hx, h(chartX.retract(x, dx)));
TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
dx(j) = 0;
H.col(j) << (dy1 - dy2) * factor;
}
@ -153,7 +157,7 @@ Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
/** use a raw C++ function pointer */
template<class Y, class X>
Matrix numericalDerivative11(Y (*h)(const X&), const X& x,
typename internal::FixedSizeMatrix<Y,X>::type numericalDerivative11(Y (*h)(const X&), const X& x,
double delta = 1e-5) {
return numericalDerivative11<Y, X>(boost::bind(h, _1), x, delta);
}
@ -167,18 +171,18 @@ Matrix numericalDerivative11(Y (*h)(const X&), const X& x,
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2>
Matrix numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(const boost::function<Y(const X1&, const X2&)>& h,
const X1& x1, const X2& x2, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X1>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2), x1, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative21<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
}
@ -192,18 +196,18 @@ inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1,
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2>
Matrix numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X2>::value,
"Template argument X2 must be a manifold type.");
// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
// "Template argument X1 must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1), x2, delta);
}
/** use a raw C++ function pointer */
template<class Y, class X1, class X2>
inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
const X2& x2, double delta = 1e-5) {
return numericalDerivative22<Y, X1, X2>(boost::bind(h, _1, _2), x1, x2, delta);
}
@ -219,18 +223,18 @@ inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1,
* All classes Y,X1,X2,X3 need dim, expmap, logmap
*/
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative31(
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X1>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X1>(boost::bind(h, _1, x2, x3), x1, delta);
}
template<class Y, class X1, class X2, class X3>
inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative31<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
@ -247,18 +251,18 @@ inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&),
* All classes Y,X1,X2,X3 need dim, expmap, logmap
*/
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative32(
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X2>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3), x2, delta);
}
template<class Y, class X1, class X2, class X3>
inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
inline typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative32<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
@ -275,18 +279,18 @@ inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&),
* All classes Y,X1,X2,X3 need dim, expmap, logmap
*/
template<class Y, class X1, class X2, class X3>
Matrix numericalDerivative33(
typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(
boost::function<Y(const X1&, const X2&, const X3&)> h, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<Y>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<Y>::structure_category>::value),
"Template argument Y must be a manifold type.");
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X3>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X3>::structure_category>::value),
"Template argument X3 must be a manifold type.");
return numericalDerivative11<Y, X3>(boost::bind(h, x1, x2, _1), x3, delta);
}
template<class Y, class X1, class X2, class X3>
inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
inline typename internal::FixedSizeMatrix<Y,X3>::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalDerivative33<Y, X1, X2, X3>(boost::bind(h, _1, _2, _3), x1,
x2, x3, delta);
@ -301,19 +305,20 @@ inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&),
* @return n*n Hessian matrix computed via central differencing
*/
template<class X>
inline Matrix numericalHessian(boost::function<double(const X&)> f, const X& x,
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(boost::function<double(const X&)> f, const X& x,
double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits<X>::structure_category>::value),
"Template argument X must be a manifold type.");
typedef Eigen::Matrix<double, traits<X>::dimension, 1> VectorD;
typedef boost::function<double(const X&)> F;
typedef boost::function<Vector(F, const X&, double)> G;
typedef boost::function<VectorD(F, const X&, double)> G;
G ng = static_cast<G>(numericalGradient<X> );
return numericalDerivative11<Vector, X>(boost::bind(ng, f, _1, delta), x,
return numericalDerivative11<VectorD, X>(boost::bind(ng, f, _1, delta), x,
delta);
}
template<class X>
inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta =
inline typename internal::FixedSizeMatrix<X,X>::type numericalHessian(double (*f)(const X&), const X& x, double delta =
1e-5) {
return numericalHessian(boost::function<double(const X&)>(f), x, delta);
}
@ -327,6 +332,8 @@ class G_x1 {
X1 x1_;
double delta_;
public:
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
G_x1(const boost::function<double(const X1&, const X2&)>& f, const X1& x1,
double delta) :
f_(f), x1_(x1), delta_(delta) {
@ -337,9 +344,10 @@ public:
};
template<class X1, class X2>
inline Matrix numericalHessian212(
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
G_x1<X1, X2> g_x1(f, x1, delta);
return numericalDerivative11<Vector, X2>(
boost::function<Vector(const X2&)>(
@ -347,17 +355,19 @@ inline Matrix numericalHessian212(
}
template<class X1, class X2>
inline Matrix numericalHessian212(double (*f)(const X1&, const X2&),
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian212(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian212(boost::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
template<class X1, class X2>
inline Matrix numericalHessian211(
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, _1, x2));
@ -368,17 +378,17 @@ inline Matrix numericalHessian211(
}
template<class X1, class X2>
inline Matrix numericalHessian211(double (*f)(const X1&, const X2&),
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian211(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian211(boost::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
}
template<class X1, class X2>
inline Matrix numericalHessian222(
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(
boost::function<double(const X1&, const X2&)> f, const X1& x1, const X2& x2,
double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, x1, _1));
@ -389,7 +399,7 @@ inline Matrix numericalHessian222(
}
template<class X1, class X2>
inline Matrix numericalHessian222(double (*f)(const X1&, const X2&),
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian222(double (*f)(const X1&, const X2&),
const X1& x1, const X2& x2, double delta = 1e-5) {
return numericalHessian222(boost::function<double(const X1&, const X2&)>(f),
x1, x2, delta);
@ -400,10 +410,10 @@ inline Matrix numericalHessian222(double (*f)(const X1&, const X2&),
*/
/* **************************************************************** */
template<class X1, class X2, class X3>
inline Matrix numericalHessian311(
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X1>::type Vector;
Vector (*numGrad)(boost::function<double(const X1&)>, const X1&,
double) = &numericalGradient<X1>;
boost::function<double(const X1&)> f2(boost::bind(f, _1, x2, x3));
@ -414,7 +424,7 @@ inline Matrix numericalHessian311(
}
template<class X1, class X2, class X3>
inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
inline typename internal::FixedSizeMatrix<X1,X1>::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian311(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@ -423,10 +433,10 @@ inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&),
/* **************************************************************** */
template<class X1, class X2, class X3>
inline Matrix numericalHessian322(
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X2>::type Vector;
Vector (*numGrad)(boost::function<double(const X2&)>, const X2&,
double) = &numericalGradient<X2>;
boost::function<double(const X2&)> f2(boost::bind(f, x1, _1, x3));
@ -437,7 +447,7 @@ inline Matrix numericalHessian322(
}
template<class X1, class X2, class X3>
inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
inline typename internal::FixedSizeMatrix<X2,X2>::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian322(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@ -446,10 +456,10 @@ inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&),
/* **************************************************************** */
template<class X1, class X2, class X3>
inline Matrix numericalHessian333(
inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
typedef typename internal::FixedSizeMatrix<X3>::type Vector;
Vector (*numGrad)(boost::function<double(const X3&)>, const X3&,
double) = &numericalGradient<X3>;
boost::function<double(const X3&)> f2(boost::bind(f, x1, x2, _1));
@ -460,7 +470,7 @@ inline Matrix numericalHessian333(
}
template<class X1, class X2, class X3>
inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
inline typename internal::FixedSizeMatrix<X3,X3>::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian333(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@ -469,7 +479,7 @@ inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&),
/* **************************************************************** */
template<class X1, class X2, class X3>
inline Matrix numericalHessian312(
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X2>(
@ -478,7 +488,7 @@ inline Matrix numericalHessian312(
}
template<class X1, class X2, class X3>
inline Matrix numericalHessian313(
inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X1, X3>(
@ -487,7 +497,7 @@ inline Matrix numericalHessian313(
}
template<class X1, class X2, class X3>
inline Matrix numericalHessian323(
inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(
boost::function<double(const X1&, const X2&, const X3&)> f, const X1& x1,
const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian212<X2, X3>(
@ -497,7 +507,7 @@ inline Matrix numericalHessian323(
/* **************************************************************** */
template<class X1, class X2, class X3>
inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
inline typename internal::FixedSizeMatrix<X1,X2>::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian312(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@ -505,7 +515,7 @@ inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&),
}
template<class X1, class X2, class X3>
inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
inline typename internal::FixedSizeMatrix<X1,X3>::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian313(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
@ -513,7 +523,7 @@ inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&),
}
template<class X1, class X2, class X3>
inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
inline typename internal::FixedSizeMatrix<X2,X3>::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&),
const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) {
return numericalHessian323(
boost::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,

80
gtsam/base/testLie.h Normal file
View File

@ -0,0 +1,80 @@
/* ----------------------------------------------------------------------------
* 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 chartTesting.h
* @brief
* @date November, 2014
* @author Paul Furgale
*/
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestResult.h>
#include <CppUnitLite/Test.h>
#include <CppUnitLite/Failure.h>
namespace gtsam {
// Do a comprehensive test of Lie Group derivatives
template<typename G>
void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
const G& t1, const G& t2) {
Matrix H1, H2;
typedef traits<G> T;
// Inverse
EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1)));
EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1));
EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1)));
EXPECT(assert_equal(numericalDerivative11(T::Inverse, t2),H1));
// Compose
EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2));
// Between
EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2));
}
// Do a comprehensive test of Lie Group Chart derivatives
template<typename G>
void testChartDerivatives(TestResult& result_, const std::string& name_,
const G& t1, const G& t2) {
Matrix H1, H2;
typedef traits<G> T;
// Retract
typename G::TangentVector w12 = T::Local(t1, t2);
EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1));
EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2));
// Local
EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1));
EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2));
}
} // namespace gtsam
#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \
{ gtsam::testLieGroupDerivatives(result_, name_, t1, t2); }
#define CHECK_CHART_DERIVATIVES(t1,t2) \
{ gtsam::testChartDerivatives(result_, name_, t1, t2); }

8
gtsam/base/tests/testLieMatrix.cpp
View File

@ -30,7 +30,7 @@ TEST( LieMatrix, construction ) {
Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished();
LieMatrix lie1(m), lie2(m);
EXPECT(lie1.dim() == 4);
EXPECT(traits<LieMatrix>::GetDimension(m) == 4);
EXPECT(assert_equal(m, lie1.matrix()));
EXPECT(assert_equal(lie1, lie2));
}
@ -50,17 +50,17 @@ TEST(LieMatrix, retract) {
Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished();
LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished());
LieMatrix actual = init.retract(update);
LieMatrix actual = traits<LieMatrix>::Retract(init,update);
EXPECT(assert_equal(expected, actual));
Vector expectedUpdate = update;
Vector actualUpdate = init.localCoordinates(actual);
Vector actualUpdate = traits<LieMatrix>::Local(init,actual);
EXPECT(assert_equal(expectedUpdate, actualUpdate));
Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished();
Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished()));
Vector actualLogmap = traits<LieMatrix>::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished()));
EXPECT(assert_equal(expectedLogmap, actualLogmap));
}

19
gtsam/base/tests/testLieScalar.cpp
View File

@ -27,6 +27,20 @@ GTSAM_CONCEPT_LIE_INST(LieScalar)
const double tol=1e-9;
//******************************************************************************
TEST(LieScalar , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<LieScalar>));
BOOST_CONCEPT_ASSERT((IsManifold<LieScalar>));
BOOST_CONCEPT_ASSERT((IsLieGroup<LieScalar>));
}
//******************************************************************************
TEST(LieScalar , Invariants) {
LieScalar lie1(2), lie2(3);
check_group_invariants(lie1, lie2);
check_manifold_invariants(lie1, lie2);
}
/* ************************************************************************* */
TEST( testLieScalar, construction ) {
double d = 2.;
@ -34,7 +48,7 @@ TEST( testLieScalar, construction ) {
EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol);
EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol);
EXPECT(lie1.dim() == 1);
EXPECT(traits<LieScalar>::dimension == 1);
EXPECT(assert_equal(lie1, lie2));
}
@ -42,7 +56,8 @@ TEST( testLieScalar, construction ) {
TEST( testLieScalar, localCoordinates ) {
LieScalar lie1(1.), lie2(3.);
EXPECT(assert_equal((Vector)(Vector(1) << 2).finished(), lie1.localCoordinates(lie2)));
Vector1 actual = traits<LieScalar>::Local(lie1, lie2);
EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual));
}
/* ************************************************************************* */

28
gtsam/base/tests/testLieVector.cpp
View File

@ -25,7 +25,21 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(LieVector)
GTSAM_CONCEPT_LIE_INST(LieVector)
/* ************************************************************************* */
//******************************************************************************
TEST(LieVector , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<LieVector>));
BOOST_CONCEPT_ASSERT((IsManifold<LieVector>));
BOOST_CONCEPT_ASSERT((IsLieGroup<LieVector>));
}
//******************************************************************************
TEST(LieVector , Invariants) {
Vector v = Vector3(1.0, 2.0, 3.0);
LieVector lie1(v), lie2(v);
check_manifold_invariants(lie1, lie2);
}
//******************************************************************************
TEST( testLieVector, construction ) {
Vector v = Vector3(1.0, 2.0, 3.0);
LieVector lie1(v), lie2(v);
@ -35,17 +49,19 @@ TEST( testLieVector, construction ) {
EXPECT(assert_equal(lie1, lie2));
}
/* ************************************************************************* */
//******************************************************************************
TEST( testLieVector, other_constructors ) {
Vector init = Vector2(10.0, 20.0);
LieVector exp(init);
double data[] = {10,20};
LieVector b(2,data);
double data[] = { 10, 20 };
LieVector b(2, data);
EXPECT(assert_equal(exp, b));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

134
gtsam/base/tests/testMatrix.cpp
View File

@ -16,12 +16,14 @@
* @author Carlos Nieto
**/
#include <iostream>
#include <sstream>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp>
#include <gtsam/base/Matrix.h>
#include <iostream>
#include <sstream>
using namespace std;
using namespace gtsam;
@ -30,7 +32,7 @@ static double inf = std::numeric_limits<double>::infinity();
static const double tol = 1e-9;
/* ************************************************************************* */
TEST( matrix, constructor_data )
TEST(Matrix, constructor_data )
{
Matrix A = (Matrix(2, 2) << -5, 3, 0, -5).finished();
@ -44,7 +46,7 @@ TEST( matrix, constructor_data )
}
/* ************************************************************************* */
TEST( matrix, Matrix_ )
TEST(Matrix, Matrix_ )
{
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0).finished();
Matrix B(2, 2);
@ -74,7 +76,7 @@ namespace {
}
/* ************************************************************************* */
TEST( matrix, special_comma_initializer)
TEST(Matrix, special_comma_initializer)
{
Matrix expected(2,2);
expected(0,0) = 1;
@ -103,7 +105,7 @@ TEST( matrix, special_comma_initializer)
}
/* ************************************************************************* */
TEST( matrix, col_major )
TEST(Matrix, col_major )
{
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
const double * const a = &A(0, 0);
@ -114,7 +116,7 @@ TEST( matrix, col_major )
}
/* ************************************************************************* */
TEST( matrix, collect1 )
TEST(Matrix, collect1 )
{
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
@ -131,7 +133,7 @@ TEST( matrix, collect1 )
}
/* ************************************************************************* */
TEST( matrix, collect2 )
TEST(Matrix, collect2 )
{
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
@ -151,7 +153,7 @@ TEST( matrix, collect2 )
}
/* ************************************************************************* */
TEST( matrix, collect3 )
TEST(Matrix, collect3 )
{
Matrix A, B;
A = eye(2, 3);
@ -168,7 +170,7 @@ TEST( matrix, collect3 )
}
/* ************************************************************************* */
TEST( matrix, stack )
TEST(Matrix, stack )
{
Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished();
Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished();
@ -191,7 +193,7 @@ TEST( matrix, stack )
}
/* ************************************************************************* */
TEST( matrix, column )
TEST(Matrix, column )
{
Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
@ -210,7 +212,7 @@ TEST( matrix, column )
}
/* ************************************************************************* */
TEST( matrix, insert_column )
TEST(Matrix, insert_column )
{
Matrix big = zeros(5, 6);
Vector col = ones(5);
@ -229,7 +231,7 @@ TEST( matrix, insert_column )
}
/* ************************************************************************* */
TEST( matrix, insert_subcolumn )
TEST(Matrix, insert_subcolumn )
{
Matrix big = zeros(5, 6);
Vector col1 = ones(2);
@ -252,7 +254,7 @@ TEST( matrix, insert_subcolumn )
}
/* ************************************************************************* */
TEST( matrix, row )
TEST(Matrix, row )
{
Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1.,
0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1.,
@ -271,7 +273,7 @@ TEST( matrix, row )
}
/* ************************************************************************* */
TEST( matrix, zeros )
TEST(Matrix, zeros )
{
Matrix A(2, 3);
A(0, 0) = 0;
@ -287,7 +289,7 @@ TEST( matrix, zeros )
}
/* ************************************************************************* */
TEST( matrix, insert_sub )
TEST(Matrix, insert_sub )
{
Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0,
1.0).finished();
@ -302,7 +304,7 @@ TEST( matrix, insert_sub )
}
/* ************************************************************************* */
TEST( matrix, diagMatrices )
TEST(Matrix, diagMatrices )
{
std::vector<Matrix> Hs;
Hs.push_back(ones(3,3));
@ -326,7 +328,7 @@ TEST( matrix, diagMatrices )
}
/* ************************************************************************* */
TEST( matrix, stream_read ) {
TEST(Matrix, stream_read ) {
Matrix expected = (Matrix(3,4) <<
1.1, 2.3, 4.2, 7.6,
-0.3, -8e-2, 5.1, 9.0,
@ -346,7 +348,7 @@ TEST( matrix, stream_read ) {
}
/* ************************************************************************* */
TEST( matrix, scale_columns )
TEST(Matrix, scale_columns )
{
Matrix A(3, 4);
A(0, 0) = 1.;
@ -384,7 +386,7 @@ TEST( matrix, scale_columns )
}
/* ************************************************************************* */
TEST( matrix, scale_rows )
TEST(Matrix, scale_rows )
{
Matrix A(3, 4);
A(0, 0) = 1.;
@ -422,7 +424,7 @@ TEST( matrix, scale_rows )
}
/* ************************************************************************* */
TEST( matrix, scale_rows_mask )
TEST(Matrix, scale_rows_mask )
{
Matrix A(3, 4);
A(0, 0) = 1.;
@ -460,7 +462,7 @@ TEST( matrix, scale_rows_mask )
}
/* ************************************************************************* */
TEST( matrix, skewSymmetric )
TEST(Matrix, skewSymmetric )
{
double wx = 1, wy = 2, wz = 3;
Matrix3 actual = skewSymmetric(wx,wy,wz);
@ -476,7 +478,7 @@ TEST( matrix, skewSymmetric )
/* ************************************************************************* */
TEST( matrix, equal )
TEST(Matrix, equal )
{
Matrix A(4, 4);
A(0, 0) = -1;
@ -506,7 +508,7 @@ TEST( matrix, equal )
}
/* ************************************************************************* */
TEST( matrix, equal_nan )
TEST(Matrix, equal_nan )
{
Matrix A(4, 4);
A(0, 0) = -1;
@ -535,7 +537,7 @@ TEST( matrix, equal_nan )
}
/* ************************************************************************* */
TEST( matrix, addition )
TEST(Matrix, addition )
{
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
@ -544,7 +546,7 @@ TEST( matrix, addition )
}
/* ************************************************************************* */
TEST( matrix, addition_in_place )
TEST(Matrix, addition_in_place )
{
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
@ -554,7 +556,7 @@ TEST( matrix, addition_in_place )
}
/* ************************************************************************* */
TEST( matrix, subtraction )
TEST(Matrix, subtraction )
{
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
@ -563,7 +565,7 @@ TEST( matrix, subtraction )
}
/* ************************************************************************* */
TEST( matrix, subtraction_in_place )
TEST(Matrix, subtraction_in_place )
{
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished();
@ -573,7 +575,7 @@ TEST( matrix, subtraction_in_place )
}
/* ************************************************************************* */
TEST( matrix, multiplication )
TEST(Matrix, multiplication )
{
Matrix A(2, 2);
A(0, 0) = -1;
@ -593,7 +595,7 @@ TEST( matrix, multiplication )
}
/* ************************************************************************* */
TEST( matrix, scalar_matrix_multiplication )
TEST(Matrix, scalar_matrix_multiplication )
{
Vector result(2);
@ -613,7 +615,7 @@ TEST( matrix, scalar_matrix_multiplication )
}
/* ************************************************************************* */
TEST( matrix, matrix_vector_multiplication )
TEST(Matrix, matrix_vector_multiplication )
{
Vector result(2);
@ -627,7 +629,7 @@ TEST( matrix, matrix_vector_multiplication )
}
/* ************************************************************************* */
TEST( matrix, nrRowsAndnrCols )
TEST(Matrix, nrRowsAndnrCols )
{
Matrix A(3, 6);
LONGS_EQUAL( A.rows() , 3 );
@ -635,7 +637,7 @@ TEST( matrix, nrRowsAndnrCols )
}
/* ************************************************************************* */
TEST( matrix, scalar_divide )
TEST(Matrix, scalar_divide )
{
Matrix A(2, 2);
A(0, 0) = 10;
@ -653,7 +655,7 @@ TEST( matrix, scalar_divide )
}
/* ************************************************************************* */
TEST( matrix, zero_below_diagonal ) {
TEST(Matrix, zero_below_diagonal ) {
Matrix A1 = (Matrix(3, 4) <<
1.0, 2.0, 3.0, 4.0,
1.0, 2.0, 3.0, 4.0,
@ -708,7 +710,7 @@ TEST( matrix, zero_below_diagonal ) {
}
/* ************************************************************************* */
TEST( matrix, inverse )
TEST(Matrix, inverse )
{
Matrix A(3, 3);
A(0, 0) = 1;
@ -754,7 +756,7 @@ TEST( matrix, inverse )
}
/* ************************************************************************* */
TEST( matrix, inverse2 )
TEST(Matrix, inverse2 )
{
Matrix A(3, 3);
A(0, 0) = 0;
@ -784,7 +786,7 @@ TEST( matrix, inverse2 )
}
/* ************************************************************************* */
TEST( matrix, backsubtitution )
TEST(Matrix, backsubtitution )
{
// TEST ONE 2x2 matrix U1*x=b1
Vector expected1 = Vector2(3.6250, -0.75);
@ -809,7 +811,7 @@ TEST( matrix, backsubtitution )
}
/* ************************************************************************* */
TEST( matrix, householder )
TEST(Matrix, householder )
{
// check in-place householder, with v vectors below diagonal
@ -838,7 +840,7 @@ TEST( matrix, householder )
}
/* ************************************************************************* */
TEST( matrix, householder_colMajor )
TEST(Matrix, householder_colMajor )
{
// check in-place householder, with v vectors below diagonal
@ -867,7 +869,7 @@ TEST( matrix, householder_colMajor )
}
/* ************************************************************************* */
TEST( matrix, eigen_QR )
TEST(Matrix, eigen_QR )
{
// use standard Eigen function to yield a non-in-place QR factorization
@ -898,7 +900,7 @@ TEST( matrix, eigen_QR )
// unit test for qr factorization (and hence householder)
// This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs
/* ************************************************************************* */
TEST( matrix, qr )
TEST(Matrix, qr )
{
Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00,
@ -921,7 +923,7 @@ TEST( matrix, qr )
}
/* ************************************************************************* */
TEST( matrix, sub )
TEST(Matrix, sub )
{
Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10,
0, 00, 10, 0, 0, 0, -10).finished();
@ -933,7 +935,7 @@ TEST( matrix, sub )
}
/* ************************************************************************* */
TEST( matrix, trans )
TEST(Matrix, trans )
{
Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0).finished();
Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
@ -941,7 +943,7 @@ TEST( matrix, trans )
}
/* ************************************************************************* */
TEST( matrix, col_major_access )
TEST(Matrix, col_major_access )
{
Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished();
const double* a = &A(0, 0);
@ -949,7 +951,7 @@ TEST( matrix, col_major_access )
}
/* ************************************************************************* */
TEST( matrix, weighted_elimination )
TEST(Matrix, weighted_elimination )
{
// create a matrix to eliminate
Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0.,
@ -984,7 +986,7 @@ TEST( matrix, weighted_elimination )
}
/* ************************************************************************* */
TEST( matrix, inverse_square_root )
TEST(Matrix, inverse_square_root )
{
Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25,
0.0, 0.0, 0.0, 0.01).finished();
@ -1036,22 +1038,22 @@ Matrix expected = (Matrix(5, 5) <<
0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished();
}
TEST( matrix, LLt )
TEST(Matrix, LLt )
{
EQUALITY(cholesky::expected, LLt(cholesky::M));
}
TEST( matrix, RtR )
TEST(Matrix, RtR )
{
EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M));
}
TEST( matrix, cholesky_inverse )
TEST(Matrix, cholesky_inverse )
{
EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M));
}
/* ************************************************************************* */
TEST( matrix, multiplyAdd )
TEST(Matrix, multiplyAdd )
{
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
@ -1062,7 +1064,7 @@ TEST( matrix, multiplyAdd )
}
/* ************************************************************************* */
TEST( matrix, transposeMultiplyAdd )
TEST(Matrix, transposeMultiplyAdd )
{
Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished();
Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.),
@ -1073,7 +1075,7 @@ TEST( matrix, transposeMultiplyAdd )
}
/* ************************************************************************* */
TEST( matrix, linear_dependent )
TEST(Matrix, linear_dependent )
{
Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
@ -1081,7 +1083,7 @@ TEST( matrix, linear_dependent )
}
/* ************************************************************************* */
TEST( matrix, linear_dependent2 )
TEST(Matrix, linear_dependent2 )
{
Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished();
@ -1089,7 +1091,7 @@ TEST( matrix, linear_dependent2 )
}
/* ************************************************************************* */
TEST( matrix, linear_dependent3 )
TEST(Matrix, linear_dependent3 )
{
Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished();
Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished();
@ -1097,7 +1099,7 @@ TEST( matrix, linear_dependent3 )
}
/* ************************************************************************* */
TEST( matrix, svd1 )
TEST(Matrix, svd1 )
{
Vector v = Vector3(2., 1., 0.);
Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1)
@ -1116,7 +1118,7 @@ static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.).finished();
static Matrix sampleAt = trans(sampleA);
/* ************************************************************************* */
TEST( matrix, svd2 )
TEST(Matrix, svd2 )
{
Matrix U, V;
Vector s;
@ -1139,7 +1141,7 @@ TEST( matrix, svd2 )
}
/* ************************************************************************* */
TEST( matrix, svd3 )
TEST(Matrix, svd3 )
{
Matrix U, V;
Vector s;
@ -1167,7 +1169,7 @@ TEST( matrix, svd3 )
}
/* ************************************************************************* */
TEST( matrix, svd4 )
TEST(Matrix, svd4 )
{
Matrix U, V;
Vector s;
@ -1209,7 +1211,7 @@ TEST( matrix, svd4 )
}
/* ************************************************************************* */
TEST( matrix, DLT )
TEST(Matrix, DLT )
{
Matrix A = (Matrix(8, 9) <<
0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11,
@ -1231,6 +1233,18 @@ TEST( matrix, DLT )
EXPECT(assert_equal(expected, actual, 1e-4));
}
//******************************************************************************
TEST(Matrix , IsVectorSpace) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix24>));
typedef Eigen::Matrix<double,2,3,Eigen::RowMajor> RowMajor;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowMajor>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Matrix>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
typedef Eigen::Matrix<double,1,-1> RowVector;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
}
/* ************************************************************************* */
int main() {
TestResult tr;

6
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -95,6 +95,12 @@ TEST(testNumericalDerivative, numericalHessian211) {
EXPECT(assert_equal(expected22, actual22, 1e-5));
}
TEST(testNumericalDerivative, numericalHessian212) {
// TODO should implement test for all the variants of numerical Hessian, for mixed dimension types,
// like Point3 y = Project(Camera, Point3);
// I'm not sure how numericalHessian212 is different from 211 or 222 -Mike B.
}
/* ************************************************************************* */
double f4(double x, double y, double z) {
return sin(x) * cos(y) * z * z;

52
gtsam/base/tests/testOptionalJacobian.cpp
View File

@ -46,6 +46,18 @@ TEST( OptionalJacobian, Constructors ) {
boost::optional<Matrix&> optional(dynamic);
OptionalJacobian<2, 3> H6(optional);
EXPECT(H6);
OptionalJacobian<-1, -1> H7;
EXPECT(!H7);
OptionalJacobian<-1, -1> H8(dynamic);
EXPECT(H8);
OptionalJacobian<-1, -1> H9(boost::none);
EXPECT(!H9);
OptionalJacobian<-1, -1> H10(optional);
EXPECT(H10);
}
//******************************************************************************
@ -59,10 +71,9 @@ void testPtr(Matrix23* H = NULL) {
*H = Matrix23::Zero();
}
TEST( OptionalJacobian, Ref2) {
TEST( OptionalJacobian, Fixed) {
Matrix expected;
expected = Matrix23::Zero();
Matrix expected = Matrix23::Zero();
// Default argument does nothing
test();
@ -88,13 +99,44 @@ TEST( OptionalJacobian, Ref2) {
Matrix dynamic1(3, 5);
dynamic1.setOnes();
test(dynamic1);
EXPECT(assert_equal(expected,dynamic0));
EXPECT(assert_equal(expected,dynamic1));
// Dynamic right size
Matrix dynamic2(2, 5);
dynamic2.setOnes();
test(dynamic2);
EXPECT(assert_equal(dynamic2,dynamic0));
EXPECT(assert_equal(expected, dynamic2));
}
//******************************************************************************
void test2(OptionalJacobian<-1,-1> H = boost::none) {
if (H)
*H = Matrix23::Zero(); // resizes
}
TEST( OptionalJacobian, Dynamic) {
Matrix expected = Matrix23::Zero();
// Default argument does nothing
test2();
// Empty is no longer a sign we don't want a matrix, we want it resized
Matrix dynamic0;
test2(dynamic0);
EXPECT(assert_equal(expected,dynamic0));
// Dynamic wrong size
Matrix dynamic1(3, 5);
dynamic1.setOnes();
test2(dynamic1);
EXPECT(assert_equal(expected,dynamic1));
// Dynamic right size
Matrix dynamic2(2, 5);
dynamic2.setOnes();
test2(dynamic2);
EXPECT(assert_equal(expected, dynamic2));
}
//******************************************************************************

60
gtsam/base/tests/testVector.cpp
View File

@ -15,10 +15,12 @@
* @author Frank Dellaert
**/
#include <iostream>
#include <gtsam/base/Vector.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/Vector.h>
#include <iostream>
using namespace std;
using namespace gtsam;
@ -40,7 +42,7 @@ namespace {
}
/* ************************************************************************* */
TEST( TestVector, special_comma_initializer)
TEST(Vector, special_comma_initializer)
{
Vector expected(3);
expected(0) = 1;
@ -68,7 +70,7 @@ TEST( TestVector, special_comma_initializer)
}
/* ************************************************************************* */
TEST( TestVector, copy )
TEST(Vector, copy )
{
Vector a(2); a(0) = 10; a(1) = 20;
double data[] = {10,20};
@ -78,14 +80,14 @@ TEST( TestVector, copy )
}
/* ************************************************************************* */
TEST( TestVector, zero1 )
TEST(Vector, zero1 )
{
Vector v = Vector::Zero(2);
EXPECT(zero(v));
}
/* ************************************************************************* */
TEST( TestVector, zero2 )
TEST(Vector, zero2 )
{
Vector a = zero(2);
Vector b = Vector::Zero(2);
@ -94,7 +96,7 @@ TEST( TestVector, zero2 )
}
/* ************************************************************************* */
TEST( TestVector, scalar_multiply )
TEST(Vector, scalar_multiply )
{
Vector a(2); a(0) = 10; a(1) = 20;
Vector b(2); b(0) = 1; b(1) = 2;
@ -102,7 +104,7 @@ TEST( TestVector, scalar_multiply )
}
/* ************************************************************************* */
TEST( TestVector, scalar_divide )
TEST(Vector, scalar_divide )
{
Vector a(2); a(0) = 10; a(1) = 20;
Vector b(2); b(0) = 1; b(1) = 2;
@ -110,7 +112,7 @@ TEST( TestVector, scalar_divide )
}
/* ************************************************************************* */
TEST( TestVector, negate )
TEST(Vector, negate )
{
Vector a(2); a(0) = 10; a(1) = 20;
Vector b(2); b(0) = -10; b(1) = -20;
@ -118,7 +120,7 @@ TEST( TestVector, negate )
}
/* ************************************************************************* */
TEST( TestVector, sub )
TEST(Vector, sub )
{
Vector a(6);
a(0) = 10; a(1) = 20; a(2) = 3;
@ -134,7 +136,7 @@ TEST( TestVector, sub )
}
/* ************************************************************************* */
TEST( TestVector, subInsert )
TEST(Vector, subInsert )
{
Vector big = zero(6),
small = ones(3);
@ -148,7 +150,7 @@ TEST( TestVector, subInsert )
}
/* ************************************************************************* */
TEST( TestVector, householder )
TEST(Vector, householder )
{
Vector x(4);
x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1;
@ -163,7 +165,7 @@ TEST( TestVector, householder )
}
/* ************************************************************************* */
TEST( TestVector, concatVectors)
TEST(Vector, concatVectors)
{
Vector A(2);
for(int i = 0; i < 2; i++)
@ -187,7 +189,7 @@ TEST( TestVector, concatVectors)
}
/* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse )
TEST(Vector, weightedPseudoinverse )
{
// column from a matrix
Vector x(2);
@ -213,7 +215,7 @@ TEST( TestVector, weightedPseudoinverse )
}
/* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse_constraint )
TEST(Vector, weightedPseudoinverse_constraint )
{
// column from a matrix
Vector x(2);
@ -238,7 +240,7 @@ TEST( TestVector, weightedPseudoinverse_constraint )
}
/* ************************************************************************* */
TEST( TestVector, weightedPseudoinverse_nan )
TEST(Vector, weightedPseudoinverse_nan )
{
Vector a = (Vector(4) << 1., 0., 0., 0.).finished();
Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished();
@ -252,7 +254,7 @@ TEST( TestVector, weightedPseudoinverse_nan )
}
/* ************************************************************************* */
TEST( TestVector, ediv )
TEST(Vector, ediv )
{
Vector a = Vector3(10., 20., 30.);
Vector b = Vector3(2.0, 5.0, 6.0);
@ -263,7 +265,7 @@ TEST( TestVector, ediv )
}
/* ************************************************************************* */
TEST( TestVector, dot )
TEST(Vector, dot )
{
Vector a = Vector3(10., 20., 30.);
Vector b = Vector3(2.0, 5.0, 6.0);
@ -271,7 +273,7 @@ TEST( TestVector, dot )
}
/* ************************************************************************* */
TEST( TestVector, axpy )
TEST(Vector, axpy )
{
Vector x = Vector3(10., 20., 30.);
Vector y0 = Vector3(2.0, 5.0, 6.0);
@ -284,7 +286,7 @@ TEST( TestVector, axpy )
}
/* ************************************************************************* */
TEST( TestVector, equals )
TEST(Vector, equals )
{
Vector v1 = (Vector(1) << 0.0/std::numeric_limits<double>::quiet_NaN()).finished(); //testing nan
Vector v2 = (Vector(1) << 1.0).finished();
@ -293,7 +295,7 @@ TEST( TestVector, equals )
}
/* ************************************************************************* */
TEST( TestVector, greater_than )
TEST(Vector, greater_than )
{
Vector v1 = Vector3(1.0, 2.0, 3.0),
v2 = zero(3);
@ -302,14 +304,14 @@ TEST( TestVector, greater_than )
}
/* ************************************************************************* */
TEST( TestVector, reciprocal )
TEST(Vector, reciprocal )
{
Vector v = Vector3(1.0, 2.0, 4.0);
EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v)));
}
/* ************************************************************************* */
TEST( TestVector, linear_dependent )
TEST(Vector, linear_dependent )
{
Vector v1 = Vector3(1.0, 2.0, 3.0);
Vector v2 = Vector3(-2.0, -4.0, -6.0);
@ -317,7 +319,7 @@ TEST( TestVector, linear_dependent )
}
/* ************************************************************************* */
TEST( TestVector, linear_dependent2 )
TEST(Vector, linear_dependent2 )
{
Vector v1 = Vector3(0.0, 2.0, 0.0);
Vector v2 = Vector3(0.0, -4.0, 0.0);
@ -325,13 +327,21 @@ TEST( TestVector, linear_dependent2 )
}
/* ************************************************************************* */
TEST( TestVector, linear_dependent3 )
TEST(Vector, linear_dependent3 )
{
Vector v1 = Vector3(0.0, 2.0, 0.0);
Vector v2 = Vector3(0.1, -4.1, 0.0);
EXPECT(!linear_dependent(v1, v2));
}
//******************************************************************************
TEST(Vector, IsVectorSpace) {
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector>));
typedef Eigen::Matrix<double,1,-1> RowVector;
BOOST_CONCEPT_ASSERT((IsVectorSpace<RowVector>));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

3
gtsam/discrete/DecisionTreeFactor.h
View File

@ -167,4 +167,7 @@ namespace gtsam {
};
// DecisionTreeFactor
// traits
template<> struct traits<DecisionTreeFactor> : public Testable<DecisionTreeFactor> {};
}// namespace gtsam

5
gtsam/discrete/DiscreteBayesNet.h
View File

@ -95,5 +95,8 @@ namespace gtsam {
}
};
} // namespace
// traits
template<> struct traits<DiscreteBayesNet> : public Testable<DiscreteBayesNet> {};
} // \ namespace gtsam

3
gtsam/discrete/DiscreteConditional.h
View File

@ -130,6 +130,9 @@ public:
};
// DiscreteConditional
// traits
template<> struct traits<DiscreteConditional> : public Testable<DiscreteConditional> {};
/* ************************************************************************* */
template<typename ITERATOR>
DiscreteConditional::shared_ptr DiscreteConditional::Combine(

5
gtsam/discrete/DiscreteFactor.h
View File

@ -20,6 +20,7 @@
#include <gtsam/discrete/Assignment.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/base/Testable.h>
namespace gtsam {
@ -101,4 +102,8 @@ public:
};
// DiscreteFactor
// traits
template<> struct traits<DiscreteFactor> : public Testable<DiscreteFactor> {};
template<> struct traits<DiscreteFactor::Values> : public Testable<DiscreteFactor::Values> {};
}// namespace gtsam

9
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -144,7 +144,10 @@ public:
//
// /** Apply a reduction, which is a remapping of variable indices. */
// GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction);
};
// DiscreteFactorGraph
} // namespace gtsam
}; // \ DiscreteFactorGraph
/// traits
template<> struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};
} // \ namespace gtsam

5
gtsam/discrete/Potentials.h
View File

@ -88,4 +88,9 @@ namespace gtsam {
}; // Potentials
// traits
template<> struct traits<Potentials> : public Testable<Potentials> {};
template<> struct traits<Potentials::ADT> : public Testable<Potentials::ADT> {};
} // namespace gtsam

5
gtsam/discrete/tests/testAlgebraicDecisionTree.cpp
View File

@ -40,6 +40,11 @@ using namespace gtsam;
/* ******************************************************************************** */
typedef AlgebraicDecisionTree<Key> ADT;
// traits
namespace gtsam {
template<> struct traits<ADT> : public Testable<ADT> {};
}
#define DISABLE_DOT
template<typename T>

10
gtsam/discrete/tests/testDecisionTree.cpp
View File

@ -44,12 +44,22 @@ void dot(const T&f, const string& filename) {
struct Crazy { int a; double b; };
typedef DecisionTree<string,Crazy> CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be)
// traits
namespace gtsam {
template<> struct traits<CrazyDecisionTree> : public Testable<CrazyDecisionTree> {};
}
/* ******************************************************************************** */
// Test string labels and int range
/* ******************************************************************************** */
typedef DecisionTree<string, int> DT;
// traits
namespace gtsam {
template<> struct traits<DT> : public Testable<DT> {};
}
struct Ring {
static inline int zero() {
return 0;

21
gtsam/geometry/Cal3Bundler.h
View File

@ -37,6 +37,8 @@ private:
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
@ -169,24 +171,7 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_manifold<Cal3Bundler> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Cal3Bundler> : public boost::integral_constant<int, 3>{
};
template<>
struct GTSAM_EXPORT zero<Cal3Bundler> {
static Cal3Bundler value() {
return Cal3Bundler(0, 0, 0);
}
};
}
struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
} // namespace gtsam

14
gtsam/geometry/Cal3DS2.h
View File

@ -43,6 +43,8 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
public:
enum { dimension = 9 };
/// @name Standard Constructors
/// @{
@ -107,18 +109,8 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_manifold<Cal3DS2> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Cal3DS2> : public boost::integral_constant<int, 9>{
};
}
struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
}

18
gtsam/geometry/Cal3Unified.h
View File

@ -50,6 +50,7 @@ private:
double xi_; // mirror parameter
public:
enum { dimension = 10 };
Vector10 vector() const ;
@ -138,23 +139,8 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_manifold<Cal3Unified> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Cal3Unified> : public boost::integral_constant<int, 10>{
};
template<>
struct GTSAM_EXPORT zero<Cal3Unified> {
static Cal3Unified value() { return Cal3Unified();}
};
}
struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
}

19
gtsam/geometry/Cal3_S2.h
View File

@ -36,7 +36,7 @@ private:
double fx_, fy_, s_, u0_, v0_;
public:
enum { dimension = 5 };
typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
/// @name Standard Constructors
@ -223,22 +223,7 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_manifold<Cal3_S2> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Cal3_S2> : public boost::integral_constant<int, 5>{
};
template<>
struct GTSAM_EXPORT zero<Cal3_S2> {
static Cal3_S2 value() { return Cal3_S2();}
};
}
struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
} // \ namespace gtsam

16
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -34,6 +34,7 @@ namespace gtsam {
public:
enum { dimension = 6 };
typedef boost::shared_ptr<Cal3_S2Stereo> shared_ptr; ///< shared pointer to stereo calibration object
/// @name Standard Constructors
@ -153,21 +154,8 @@ namespace gtsam {
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_manifold<Cal3_S2Stereo> : public boost::true_type{
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
template<>
struct GTSAM_EXPORT dimension<Cal3_S2Stereo> : public boost::integral_constant<int, 6>{
};
template<>
struct GTSAM_EXPORT zero<Cal3_S2Stereo> {
static Cal3_S2Stereo value() { return Cal3_S2Stereo();}
};
}
} // \ namespace gtsam

19
gtsam/geometry/CalibratedCamera.h
View File

@ -44,6 +44,8 @@ private:
public:
enum { dimension = 6 };
/// @name Standard Constructors
/// @{
@ -199,23 +201,8 @@ private:
/// @}
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_group<CalibratedCamera> : public boost::true_type {
};
template<>
struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type {
};
template<>
struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<
int, 6> {
};
}
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
}

20
gtsam/base/Lie-inl.h → gtsam/geometry/Cyclic.cpp
View File

@ -10,19 +10,13 @@
* -------------------------------------------------------------------------- */
/**
* @file Lie-inl.h
* @date Jan 9, 2010
* @author Richard Roberts
* @brief Instantiate macro for Lie type
*/
* @file Cyclic.cpp
* @brief Cyclic group implementation
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/base/Lie.h>
#define INSTANTIATE_LIE(T) \
template T between_default(const T&, const T&); \
template Vector logmap_default(const T&, const T&); \
template T expmap_default(const T&, const Vector&);
#include <gtsam/geometry/Cyclic.h>
namespace gtsam {
} // \namespace gtsam

83
gtsam/geometry/Cyclic.h Normal file
View File

@ -0,0 +1,83 @@
/* ----------------------------------------------------------------------------
* 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 Cyclic.h
* @brief Cyclic group, i.e., the integers modulo N
* @author Frank Dellaert
**/
#include <gtsam/base/Group.h>
#include <cstddef>
#include <string>
#include <iostream>
#include <assert.h>
namespace gtsam {
/// Cyclic group of order N
template<size_t N>
class Cyclic {
size_t i_; ///< we just use an unsigned int
public:
/// Constructor
Cyclic(size_t i) :
i_(i) {
assert(i < N);
}
/// Idenity element
static Cyclic Identity() {
return Cyclic(0);
}
/// Cast to size_t
operator size_t() const {
return i_;
}
/// Addition modulo N
Cyclic operator+(const Cyclic& h) const {
return (i_ + h.i_) % N;
}
/// Subtraction modulo N
Cyclic operator-(const Cyclic& h) const {
return (N + i_ - h.i_) % N;
}
/// Inverse
Cyclic operator-() const {
return (N - i_) % N;
}
/// print with optional string
void print(const std::string& s = "") const {
std::cout << s << i_ << std::endl;
}
/// equals with an tolerance, prints out message if unequal
bool equals(const Cyclic& other, double tol = 1e-9) const {
return other.i_ == i_;
}
};
/// Define cyclic group traits to be a model of the Group concept
template<size_t N>
struct traits<Cyclic<N> > {
typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic<N>)
static Cyclic<N> Identity() {
return Cyclic<N>::Identity();
}
static bool Equals(const Cyclic<N>& a, const Cyclic<N>& b,
double tol = 1e-9) {
return a.equals(b, tol);
}
static void Print(const Cyclic<N>& c, const std::string &s = "") {
c.print(s);
}
};
} // \namespace gtsam

19
gtsam/geometry/EssentialMatrix.h
View File

@ -30,6 +30,8 @@ private:
public:
enum { dimension = 5 };
/// Static function to convert Point2 to homogeneous coordinates
static Vector3 Homogeneous(const Point2& p) {
return Vector3(p.x(), p.y(), 1);
@ -196,23 +198,8 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_manifold<EssentialMatrix> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<EssentialMatrix> : public boost::integral_constant<int, 5>{
};
template<>
struct GTSAM_EXPORT zero<EssentialMatrix> {
static EssentialMatrix value() { return EssentialMatrix();}
};
}
struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
} // gtsam

56
gtsam/geometry/PinholeCamera.h
View File

@ -31,16 +31,20 @@ namespace gtsam {
*/
template<typename Calibration>
class PinholeCamera {
private:
Pose3 pose_;
Calibration K_;
// Get dimensions of calibration type and This at compile time
static const int DimK = traits::dimension<Calibration>::value, //
DimC = 6 + DimK;
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
// Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<Calibration>::value;
public:
enum { dimension = 6 + DimK };
/// @name Standard Constructors
/// @{
@ -168,15 +172,15 @@ public:
/// Manifold dimension
inline size_t dim() const {
return DimC;
return dimension;
}
/// Manifold dimension
inline static size_t Dim() {
return DimC;
return dimension;
}
typedef Eigen::Matrix<double, DimC, 1> VectorK6;
typedef Eigen::Matrix<double, dimension, 1> VectorK6;
/// move a cameras according to d
PinholeCamera retract(const Vector& d) const {
@ -189,12 +193,18 @@ public:
/// return canonical coordinate
VectorK6 localCoordinates(const PinholeCamera& T2) const {
VectorK6 d; // TODO: why does d.head<6>() not compile??
VectorK6 d;
// TODO: why does d.head<6>() not compile??
d.head(6) = pose().localCoordinates(T2.pose());
d.tail(DimK) = calibration().localCoordinates(T2.calibration());
return d;
}
/// for Canonical
static PinholeCamera identity() {
return PinholeCamera(); // assumes that the default constructor is valid
}
/// @}
/// @name Transformations and measurement functions
/// @{
@ -309,7 +319,7 @@ public:
*/
Point2 project2(
const Point3& pw, //
OptionalJacobian<2, DimC> Dcamera = boost::none,
OptionalJacobian<2, dimension> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw);
@ -359,7 +369,7 @@ public:
*/
double range(
const Point3& point, //
OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
@ -377,7 +387,7 @@ public:
*/
double range(
const Pose3& pose, //
OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
@ -396,7 +406,7 @@ public:
template<class CalibrationB>
double range(
const PinholeCamera<CalibrationB>& camera, //
OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, dimension> Dcamera = boost::none,
// OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
boost::optional<Matrix&> Dother =
boost::none) const {
@ -408,7 +418,7 @@ public:
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
}
if (Dother) {
Dother->resize(1, 6+traits::dimension<CalibrationB>::value);
Dother->resize(1, 6+CalibrationB::dimension);
Dother->setZero();
Dother->block(0, 0, 1, 6) = Dother_;
}
@ -424,7 +434,7 @@ public:
*/
double range(
const CalibratedCamera& camera, //
OptionalJacobian<1, DimC> Dcamera = boost::none,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
return range(camera.pose(), Dcamera, Dother);
}
@ -485,26 +495,8 @@ private:
};
// Define GTSAM traits
namespace traits {
template<typename Calibration>
struct GTSAM_EXPORT is_manifold<PinholeCamera<Calibration> > : public boost::true_type {
};
template<typename Calibration>
struct GTSAM_EXPORT dimension<PinholeCamera<Calibration> > : public boost::integral_constant<
int, dimension<Pose3>::value + dimension<Calibration>::value> {
};
template<typename Calibration>
struct GTSAM_EXPORT zero<PinholeCamera<Calibration> > {
static PinholeCamera<Calibration> value() {
return PinholeCamera<Calibration>(zero<Pose3>::value(),
zero<Calibration>::value());
}
};
}
struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
} // \ gtsam

4
gtsam/geometry/Point2.cpp
View File

@ -16,7 +16,6 @@
*/
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Lie-inl.h>
#include <boost/foreach.hpp>
#include <cmath>
@ -24,9 +23,6 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point2);
/* ************************************************************************* */
void Point2::print(const string& s) const {
cout << s << *this << endl;

103
gtsam/geometry/Point2.h
View File

@ -17,12 +17,9 @@
#pragma once
#include <gtsam/base/VectorSpace.h>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Lie.h>
namespace gtsam {
/**
@ -33,13 +30,12 @@ namespace gtsam {
* \nosubgrouping
*/
class GTSAM_EXPORT Point2 {
private:
double x_, y_;
public:
enum { dimension = 2 };
/// @name Standard Constructors
/// @{
@ -54,9 +50,7 @@ public:
/// @{
/// construct from 2D vector
Point2(const Vector& v) {
if(v.size() != 2)
throw std::invalid_argument("Point2 constructor from Vector requires that the Vector have dimension 2");
Point2(const Vector2& v) {
x_ = v(0);
y_ = v(1);
}
@ -113,72 +107,17 @@ public:
/// @{
/// identity
inline static Point2 identity() {
return Point2();
}
inline static Point2 identity() {return Point2();}
/// "Inverse" - negates each coordinate such that compose(p,inverse(p)) == identity()
inline Point2 inverse() const { return Point2(-x_, -y_); }
/// syntactic sugar for inverse, i.e., -p == inverse(p)
/// inverse
inline Point2 operator- () const {return Point2(-x_,-y_);}
/// "Compose", just adds the coordinates of two points. With optional derivatives
inline Point2 compose(const Point2& q,
OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = I_2x2;
if(H2) *H2 = I_2x2;
return *this + q;
}
/// syntactic sugar for adding two points, i.e., p+q == compose(p,q)
/// add
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
/// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q)
inline Point2 between(const Point2& q,
OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = -I_2x2;
if(H2) *H2 = I_2x2;
return q - (*this);
}
/// syntactic sugar for subtracting points, i.e., q-p == between(p,q)
/// subtract
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
/// @}
/// @name Manifold
/// @{
/// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return 2; }
/// Dimensionality of tangent space = 2 DOF
inline size_t dim() const { return 2; }
/// Updates a with tangent space delta
inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
/// Local coordinates of manifold neighborhood around current value
inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); }
/// @}
/// @name Lie Group
/// @{
/// Exponential map around identity - just create a Point2 from a vector
static inline Point2 Expmap(const Vector& v) { return Point2(v); }
/// Log map around identity - just return the Point2 as a vector
static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); }
/// Left-trivialized derivative of the exponential map
static Matrix ExpmapDerivative(const Vector2& v) {return I_2x2;}
/// Left-trivialized derivative inverse of the exponential map
static Matrix LogmapDerivative(const Vector2& v) { return I_2x2;}
/// @}
/// @name Vector Space
/// @{
@ -221,10 +160,18 @@ public:
Vector2 vector() const { return Vector2(x_, y_); }
/// @}
/// @name Deprecated (non-const, non-functional style. Do not use).
/// @name Deprecated
/// @{
inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
inline void operator *= (double s) {x_*=s;y_*=s;}
Point2 inverse() { return -(*this);}
Point2 compose(const Point2& q) { return (*this)+q;}
Point2 between(const Point2& q) { return q-(*this);}
Vector2 localCoordinates(const Point2& q) { return between(q).vector();}
Point2 retract(const Vector2& v) {return compose(Point2(v));}
static Vector2 Logmap(const Point2& p) {return p.vector();}
static Point2 Expmap(const Vector2& v) { return Point2(v);}
/// @}
/// Streaming
@ -251,22 +198,8 @@ private:
/// multiply with scalar
inline Point2 operator*(double s, const Point2& p) {return p*s;}
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_group<Point2> : public boost::true_type{
};
struct traits<Point2> : public internal::VectorSpace<Point2> {};
template<>
struct GTSAM_EXPORT is_manifold<Point2> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Point2> : public boost::integral_constant<int, 2>{
};
}
}
} // \ namespace gtsam

4
gtsam/geometry/Point3.cpp
View File

@ -15,15 +15,11 @@
*/
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/Lie-inl.h>
using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point3);
/* ************************************************************************* */
bool Point3::equals(const Point3 & q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol

105
gtsam/geometry/Point3.h
View File

@ -21,12 +21,8 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
#include <boost/serialization/nvp.hpp>
#include <cmath>
namespace gtsam {
@ -43,6 +39,7 @@ namespace gtsam {
double x_, y_, z_;
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
@ -58,9 +55,7 @@ namespace gtsam {
/// @{
/// Construct from 3-element vector
Point3(const Vector& v) {
if(v.size() != 3)
throw std::invalid_argument("Point3 constructor from Vector requires that the Vector have dimension 3");
Point3(const Vector3& v) {
x_ = v(0);
y_ = v(1);
z_ = v(2);
@ -81,76 +76,17 @@ namespace gtsam {
/// @{
/// identity for group operation
inline static Point3 identity() {
return Point3();
}
inline static Point3 identity() { return Point3();}
/// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3()
inline Point3 inverse() const { return Point3(-x_, -y_, -z_); }
/// syntactic sugar for inverse, i.e., -p == inverse(p)
/// inverse
Point3 operator - () const { return Point3(-x_,-y_,-z_);}
/// "Compose" - just adds coordinates of two points
inline Point3 compose(const Point3& p2,
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const {
if (H1) *H1 << I_3x3;
if (H2) *H2 << I_3x3;
return *this + p2;
}
///syntactic sugar for adding two points, i.e., p+q == compose(p,q)
/// add
Point3 operator + (const Point3& q) const;
/** Between using the default implementation */
inline Point3 between(const Point3& p2,
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const {
if(H1) *H1 = -I_3x3;
if(H2) *H2 = I_3x3;
return p2 - *this;
}
/// syntactic sugar for subtracting points, i.e., q-p == between(p,q)
/// subtract
Point3 operator - (const Point3& q) const;
/// @}
/// @name Manifold
/// @{
/// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return 3; }
/// return dimensionality of tangent space, DOF = 3
inline size_t dim() const { return 3; }
/// Updates a with tangent space delta
inline Point3 retract(const Vector& v) const { return Point3(*this + v); }
/// Returns inverse retraction
inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); }
/// @}
/// @name Lie Group
/// @{
/** Exponential map at identity - just create a Point3 from x,y,z */
static inline Point3 Expmap(const Vector& v) { return Point3(v); }
/** Log map at identity - return the x,y,z of this point */
static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
/// Left-trivialized derivative of the exponential map
static Matrix3 ExpmapDerivative(const Vector& v) {
return I_3x3;
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 LogmapDerivative(const Vector& v) {
return I_3x3;
}
/// @}
/// @name Vector Space
/// @{
@ -226,6 +162,17 @@ namespace gtsam {
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
/// @name Deprecated
/// @{
Point3 inverse() { return -(*this);}
Point3 compose(const Point3& q) { return (*this)+q;}
Point3 between(const Point3& q) { return q-(*this);}
Vector3 localCoordinates(const Point3& q) { return between(q).vector();}
Point3 retract(const Vector3& v) {return compose(Point3(v));}
static Vector3 Logmap(const Point3& p) {return p.vector();}
static Point3 Expmap(const Vector3& v) { return Point3(v);}
/// @}
private:
/// @name Advanced Interface
@ -248,20 +195,6 @@ namespace gtsam {
/// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;}
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_group<Point3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT is_manifold<Point3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Point3> : public boost::integral_constant<int, 3>{
};
}
struct traits<Point3> : public internal::VectorSpace<Point3> {};
}

84
gtsam/geometry/Pose2.cpp
View File

@ -16,9 +16,11 @@
#include <gtsam/geometry/concepts.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/concepts.h>
#include <boost/foreach.hpp>
#include <cmath>
#include <iostream>
#include <iomanip>
@ -27,9 +29,6 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose2);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2);
@ -60,7 +59,8 @@ bool Pose2::equals(const Pose2& q, double tol) const {
}
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi) {
Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) {
if (H) *H = Pose2::ExpmapDerivative(xi);
assert(xi.size() == 3);
Point2 v(xi(0),xi(1));
double w = xi(2);
@ -75,7 +75,8 @@ Pose2 Pose2::Expmap(const Vector& xi) {
}
/* ************************************************************************* */
Vector3 Pose2::Logmap(const Pose2& p) {
Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) {
if (H) *H = Pose2::LogmapDerivative(p);
const Rot2& R = p.r();
const Point2& t = p.t();
double w = R.theta();
@ -91,21 +92,26 @@ Vector3 Pose2::Logmap(const Pose2& p) {
}
/* ************************************************************************* */
Pose2 Pose2::retract(const Vector& v) const {
Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) {
#ifdef SLOW_BUT_CORRECT_EXPMAP
return compose(Expmap(v));
return Expmap(v, H);
#else
assert(v.size() == 3);
return compose(Pose2(v[0], v[1], v[2]));
if (H) {
*H = I_3x3;
H->topLeftCorner<2,2>() = Rot2(-v[2]).matrix();
}
return Pose2(v[0], v[1], v[2]);
#endif
}
/* ************************************************************************* */
Vector3 Pose2::localCoordinates(const Pose2& p2) const {
Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) {
#ifdef SLOW_BUT_CORRECT_EXPMAP
return Logmap(between(p2));
return Logmap(r, H);
#else
Pose2 r = between(p2);
if (H) {
*H = I_3x3;
H->topLeftCorner<2,2>() = r.rotation().matrix();
}
return Vector3(r.x(), r.y(), r.theta());
#endif
}
@ -166,7 +172,8 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) {
}
/* ************************************************************************* */
Matrix3 Pose2::LogmapDerivative(const Vector3& v) {
Matrix3 Pose2::LogmapDerivative(const Pose2& p) {
Vector3 v = Logmap(p);
double alpha = v[2];
Matrix3 J;
if (fabs(alpha) > 1e-5) {
@ -187,8 +194,7 @@ Matrix3 Pose2::LogmapDerivative(const Vector3& v) {
}
/* ************************************************************************* */
Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -AdjointMap();
Pose2 Pose2::inverse() const {
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
@ -206,16 +212,6 @@ Point2 Pose2::transform_to(const Point2& point,
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
// TODO: inline and reuse?
if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I_3x3;
return (*this)*p2;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
@ -231,40 +227,6 @@ Point2 Pose2::transform_from(const Point2& p,
return q + t_;
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
// t = R1' * (t2-t1)
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 = I_3x3;
return Pose2(R,t);
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point,
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {

63
gtsam/geometry/Pose2.h
View File

@ -20,11 +20,9 @@
#pragma once
#include <boost/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie.h>
namespace gtsam {
@ -33,7 +31,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Pose2 {
class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
public:
@ -42,6 +40,7 @@ public:
typedef Point2 Translation;
private:
Rot2 r_;
Point2 t_;
@ -106,50 +105,23 @@ public:
/// identity for group operation
inline static Pose2 identity() { return Pose2(); }
/// inverse transformation with derivatives
Pose2 inverse(OptionalJacobian<3, 3> H1=boost::none) const;
/// compose this transformation onto another (first *this and then p2)
Pose2 compose(const Pose2& p2,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// inverse
Pose2 inverse() const;
/// compose syntactic sugar
inline Pose2 operator*(const Pose2& p2) const {
return Pose2(r_*p2.r(), t_ + r_*p2.t());
}
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H = boost::none) const;
/// @}
/// @name Manifold
/// @{
/// Dimensionality of tangent space = 3 DOF - used to autodetect sizes
inline static size_t Dim() { return 3; }
/// Dimensionality of tangent space = 3 DOF
inline size_t dim() const { return 3; }
/// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose
Pose2 retract(const Vector& v) const;
/// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
Vector3 localCoordinates(const Pose2& p2) const;
/// @}
/// @name Lie Group
/// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
static Pose2 Expmap(const Vector& xi);
static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none);
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
static Vector3 Logmap(const Pose2& p);
static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
/**
* Calculate Adjoint map
@ -185,8 +157,15 @@ public:
static Matrix3 ExpmapDerivative(const Vector3& v);
/// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& v);
static Matrix3 LogmapDerivative(const Pose2& v);
// Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
struct ChartAtOrigin {
static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
};
using LieGroup<Pose2, 3>::inverse; // version with derivative
/// @}
/// @name Group Action on Point2
@ -311,18 +290,8 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_manifold<Pose2> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Pose2> : public boost::integral_constant<int, 3>{
};
}
struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {};
} // namespace gtsam

138
gtsam/geometry/Pose3.cpp
View File

@ -17,7 +17,8 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/concepts.h>
#include <gtsam/base/Lie-inl.h>
#include <gtsam/base/concepts.h>
#include <boost/foreach.hpp>
#include <iostream>
#include <cmath>
@ -26,9 +27,6 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose3);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3);
@ -38,6 +36,12 @@ Pose3::Pose3(const Pose2& pose2) :
Point3(pose2.x(), pose2.y(), 0)) {
}
/* ************************************************************************* */
Pose3 Pose3::inverse() const {
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt * (-t_));
}
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
@ -107,7 +111,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector& xi) {
Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) {
if (H) *H = ExpmapDerivative(xi);
// get angular velocity omega and translational velocity v from twist xi
Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5));
@ -127,7 +132,8 @@ Pose3 Pose3::Expmap(const Vector& xi) {
}
/* ************************************************************************* */
Vector6 Pose3::Logmap(const Pose3& p) {
Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) {
if (H) *H = LogmapDerivative(p);
Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector();
double t = w.norm();
if (t < 1e-10) {
@ -148,57 +154,35 @@ Vector6 Pose3::Logmap(const Pose3& p) {
}
/* ************************************************************************* */
Pose3 Pose3::retractFirstOrder(const Vector& xi) const {
Vector3 omega(sub(xi, 0, 3));
Point3 v(sub(xi, 3, 6));
Rot3 R = R_.retract(omega); // R is done exactly
Point3 t = t_ + R_ * v; // First order t approximation
return Pose3(R, t);
Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
#ifdef GTSAM_POSE3_EXPMAP
return Expmap(xi, H);
#else
Matrix3 DR;
Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0);
if (H) {
*H = I_6x6;
H->topLeftCorner<3,3>() = DR;
}
return Pose3(R, Point3(xi.tail<3>()));
#endif
}
/* ************************************************************************* */
// Different versions of retract
Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const {
if (mode == Pose3::EXPMAP) {
// Lie group exponential map, traces out geodesic
return compose(Expmap(xi));
} else if (mode == Pose3::FIRST_ORDER) {
// First order
return retractFirstOrder(xi);
} else {
// Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently
// Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation
assert(false);
exit(1);
}
}
/* ************************************************************************* */
// different versions of localCoordinates
Vector6 Pose3::localCoordinates(const Pose3& T,
Pose3::CoordinatesMode mode) const {
if (mode == Pose3::EXPMAP) {
// Lie group logarithm map, exact inverse of exponential map
return Logmap(between(T));
} else if (mode == Pose3::FIRST_ORDER) {
// R is always done exactly in all three retract versions below
Vector3 omega = R_.localCoordinates(T.rotation());
// Incorrect version
// Independently computes the logmap of the translation and rotation
// Vector v = t_.localCoordinates(T.translation());
// Correct first order t inverse
Point3 d = R_.unrotate(T.translation() - t_);
// TODO: correct second order t inverse
Vector6 local;
local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z();
return local;
} else {
assert(false);
exit(1);
Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) {
#ifdef GTSAM_POSE3_EXPMAP
return Logmap(T, H);
#else
Matrix3 DR;
Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0);
if (H) {
*H = I_6x6;
H->topLeftCorner<3,3>() = DR;
}
Vector6 xi;
xi << omega, T.translation().vector();
return xi;
#endif
}
/* ************************************************************************* */
@ -231,12 +215,20 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
}
#else
// The closed-form formula in Barfoot14tro eq. (102)
double phi = w.norm(), sinPhi = sin(phi), cosPhi = cos(phi), phi2 = phi * phi,
phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
// Invert the sign of odd-order terms to have the right Jacobian
Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W)
+ (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W)
- 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W);
double phi = w.norm();
if (fabs(phi)>1e-5) {
double sinPhi = sin(phi), cosPhi = cos(phi);
double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi;
// Invert the sign of odd-order terms to have the right Jacobian
Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W)
+ (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W)
- 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W);
}
else {
Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W)
+ 1./24.*(W*W*V + V*W*W - 3*W*V*W)
- 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W);
}
#endif
return Q;
@ -252,7 +244,8 @@ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
}
/* ************************************************************************* */
Matrix6 Pose3::LogmapDerivative(const Vector6& xi) {
Matrix6 Pose3::LogmapDerivative(const Pose3& pose) {
Vector6 xi = Logmap(pose);
Vector3 w(sub(xi, 0, 3));
Matrix3 Jw = Rot3::LogmapDerivative(w);
Matrix3 Q = computeQforExpmapDerivative(xi);
@ -311,31 +304,6 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
return q;
}
/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
OptionalJacobian<6,6> H2) const {
if (H1) *H1 = p2.inverse().AdjointMap();
if (H2) *H2 = I_6x6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
if (H1) *H1 = -AdjointMap();
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt * (-t_));
}
/* ************************************************************************* */
// between = compose(p2,inverse(p1));
Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
OptionalJacobian<6,6> H2) const {
Pose3 result = inverse() * p2;
if (H1) *H1 = -result.inverse().AdjointMap();
if (H2) *H2 = I_6x6;
return result;
}
/* ************************************************************************* */
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 3> H2) const {

437
gtsam/geometry/Pose3.h
View File

@ -19,15 +19,9 @@
#include <gtsam/config.h>
#ifndef GTSAM_POSE3_EXPMAP
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
#else
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP
#endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Lie.h>
namespace gtsam {
@ -39,7 +33,7 @@ class Pose2;
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Pose3{
class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
public:
/** Pose Concept requirements */
@ -48,7 +42,7 @@ public:
private:
Rot3 R_; ///< Rotation gRp, between global and pose frame
Rot3 R_; ///< Rotation gRp, between global and pose frame
Point3 t_; ///< Translation gTp, from global origin to pose frame origin
public:
@ -75,7 +69,6 @@ public:
R_(R), t_(t) {
}
/** Construct from Pose2 */
explicit Pose3(const Pose2& pose2);
@ -105,239 +98,218 @@ public:
}
/// inverse transformation with derivatives
Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const;
Pose3 inverse() const;
/// compose syntactic sugar
Pose3 operator*(const Pose3& T) const {
return Pose3(R_ * T.R_, t_ + R_ * T.t_);
}
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives
*/
Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
OptionalJacobian<6, 6> H2 = boost::none) const;
/// @}
/// @name Manifold
/// @name Lie Group
/// @{
/** Enum to indicate which method should be used in Pose3::retract() and
* Pose3::localCoordinates()
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none);
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none);
/**
* Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
* Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi)
*/
enum CoordinatesMode {
EXPMAP, ///< The correct exponential map, computationally expensive.
FIRST_ORDER ///< A fast first-order approximation to the exponential map.
Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect
/**
* Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame
* \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
*/
Vector Adjoint(const Vector& xi_b) const {
return AdjointMap() * xi_b;
} /// FIXME Not tested - marked as incorrect
/**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
* [ad(w,v)] = [w^, zero3; v^, w^]
* Note that this is the matrix representation of the adjoint operator for se3 Lie algebra,
* aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3.
*
* Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its
* vector representation.
* We have the following relationship:
* \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$
*
* We use this to compute the discrete version of the inverse right-trivialized tangent map,
* and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
*
*/
static Matrix6 adjointMap(const Vector6 &xi);
/**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y,
OptionalJacobian<6, 6> = boost::none);
/**
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> H = boost::none);
/// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi);
/// Derivative of Logmap
static Matrix6 LogmapDerivative(const Pose3& xi);
// Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
struct ChartAtOrigin {
static Pose3 Retract(const Vector6& v, ChartJacobian H = boost::none);
static Vector6 Local(const Pose3& r, ChartJacobian H = boost::none);
};
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
static size_t Dim() {
return 6;
}
/// Dimensionality of the tangent space = 6 DOF
size_t dim() const {
return 6;
}
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
Pose3 retractFirstOrder(const Vector& d) const;
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose
Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode =
POSE3_DEFAULT_COORDINATES_MODE) const;
/// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose
Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const;
/// @}
/// @name Lie Group
/// @{
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
static Pose3 Expmap(const Vector& xi);
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
static Vector6 Logmap(const Pose3& p);
/**
* Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame
* Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi)
*/
Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect
/**
* Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame
* \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
*/
Vector Adjoint(const Vector& xi_b) const {return AdjointMap()*xi_b; } /// FIXME Not tested - marked as incorrect
/**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
* [ad(w,v)] = [w^, zero3; v^, w^]
* Note that this is the matrix representation of the adjoint operator for se3 Lie algebra,
* aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3.
*
* Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its
* vector representation.
* We have the following relationship:
* \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$
*
* We use this to compute the discrete version of the inverse right-trivialized tangent map,
* and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
*
*/
static Matrix6 adjointMap(const Vector6 &xi);
/**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none);
/**
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none);
public:
/// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi);
/// Derivative of Logmap
static Matrix6 LogmapDerivative(const Vector6& xi);
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = (wx,wy,wz) 3D angular velocity
* v (vx,vy,vz) = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) {
return (Matrix(4,4) <<
0.,-wz, wy, vx,
wz, 0.,-wx, vy,
-wy, wx, 0., vz,
0., 0., 0., 0.).finished();
}
/// @}
/// @name Group Action on Point3
/// @{
/**
* @brief takes point in Pose coordinates and transforms it to world coordinates
* @param p point in Pose coordinates
* @param Dpose optional 3*6 Jacobian wrpt this pose
* @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in world coordinates
*/
Point3 transform_from(const Point3& p,
OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const;
/** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
/**
* @brief takes point in world coordinates and transforms it to Pose coordinates
* @param p point in world coordinates
* @param Dpose optional 3*6 Jacobian wrpt this pose
* @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates
*/
Point3 transform_to(const Point3& p,
OptionalJacobian<3,6> Dpose = boost::none,
OptionalJacobian<3,3> Dpoint = boost::none) const;
/// @}
/// @name Standard Interface
/// @{
/// get rotation
const Rot3& rotation() const { return R_; }
/// get translation
const Point3& translation() const { return t_; }
/// get x
double x() const { return t_.x(); }
/// get y
double y() const { return t_.y(); }
/// get z
double z() const { return t_.z(); }
/** convert to 4*4 matrix */
Matrix4 matrix() const;
/** receives a pose in world coordinates and transforms it to local coordinates */
Pose3 transform_to(const Pose3& pose) const;
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @return range (double)
*/
double range(const Point3& point,
OptionalJacobian<1,6> H1=boost::none,
OptionalJacobian<1,3> H2=boost::none) const;
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @return range (double)
*/
double range(const Pose3& pose,
OptionalJacobian<1,6> H1=boost::none,
OptionalJacobian<1,6> H2=boost::none) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Return the start and end indices (inclusive) of the translation component of the
* exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector
*/
inline static std::pair<size_t, size_t> translationInterval() { return std::make_pair(3, 5); }
/**
* Return the start and end indices (inclusive) of the rotation component of the
* exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector
*/
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(0, 2); }
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}
/// @}
};// Pose3 class
using LieGroup<Pose3, 6>::inverse; // version with derivative
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = 3D angular velocity
* v = 3D velocity
* omega = (wx,wy,wz) 3D angular velocity
* v (vx,vy,vz) = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
static Matrix wedge(double wx, double wy, double wz, double vx, double vy,
double vz) {
return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
}
/// @}
/// @name Group Action on Point3
/// @{
/**
* @brief takes point in Pose coordinates and transforms it to world coordinates
* @param p point in Pose coordinates
* @param Dpose optional 3*6 Jacobian wrpt this pose
* @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in world coordinates
*/
Point3 transform_from(const Point3& p, OptionalJacobian<3, 6> Dpose =
boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
/** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const {
return transform_from(p);
}
/**
* @brief takes point in world coordinates and transforms it to Pose coordinates
* @param p point in world coordinates
* @param Dpose optional 3*6 Jacobian wrpt this pose
* @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates
*/
Point3 transform_to(const Point3& p, OptionalJacobian<3, 6> Dpose =
boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const;
/// @}
/// @name Standard Interface
/// @{
/// get rotation
const Rot3& rotation() const {
return R_;
}
/// get translation
const Point3& translation() const {
return t_;
}
/// get x
double x() const {
return t_.x();
}
/// get y
double y() const {
return t_.y();
}
/// get z
double z() const {
return t_.z();
}
/** convert to 4*4 matrix */
Matrix4 matrix() const;
/** receives a pose in world coordinates and transforms it to local coordinates */
Pose3 transform_to(const Pose3& pose) const;
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @return range (double)
*/
double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const;
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @return range (double)
*/
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) const;
/// @}
/// @name Advanced Interface
/// @{
/**
* Return the start and end indices (inclusive) of the translation component of the
* exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector
*/
inline static std::pair<size_t, size_t> translationInterval() {
return std::make_pair(3, 5);
}
/**
* Return the start and end indices (inclusive) of the rotation component of the
* exponential map parameterization
* @return a pair of [start, end] indices into the tangent space vector
*/
static std::pair<size_t, size_t> rotationInterval() {
return std::make_pair(0, 2);
}
/// Output stream operator
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}
/// @}
};
// Pose3 class
/**
* wedge for Pose3:
* @param xi 6-dim twist (omega,v) where
* omega = 3D angular velocity
* v = 3D velocity
* @return xihat, 4*4 element of Lie algebra that can be exponentiated
*/
template<>
inline Matrix wedge<Pose3>(const Vector& xi) {
return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
@ -350,21 +322,8 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_group<Pose3> : public boost::true_type{
struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {
};
template<>
struct GTSAM_EXPORT is_manifold<Pose3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Pose3> : public boost::integral_constant<int, 6>{
};
}
} // namespace gtsam

167
gtsam/geometry/Quaternion.h Normal file
View File

@ -0,0 +1,167 @@
/* ----------------------------------------------------------------------------
* 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 Quaternion.h
* @brief Lie Group wrapper for Eigen Quaternions
* @author Frank Dellaert
**/
#include <gtsam/base/Lie.h>
#include <gtsam/base/concepts.h>
#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
namespace gtsam {
// Define traits
template<typename _Scalar, int _Options>
struct traits<QUATERNION_TYPE> {
typedef QUATERNION_TYPE ManifoldType;
typedef QUATERNION_TYPE Q;
typedef lie_group_tag structure_category;
typedef multiplicative_group_tag group_flavor;
/// @name Group traits
/// @{
static Q Identity() {
return Q::Identity();
}
static Q Compose(const Q &g, const Q & h) {
return g * h;
}
static Q Between(const Q &g, const Q & h) {
Q d = g.inverse() * h;
return d;
}
static Q Inverse(const Q &g) {
return g.inverse();
}
/// @}
/// @name Basic manifold traits
/// @{
enum {
dimension = 3
};
typedef OptionalJacobian<3, 3> ChartJacobian;
typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
/// @}
/// @name Lie group traits
/// @{
static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
boost::none) {
if (Hg)
*Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? )
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart ( I(3)? )
return g * h;
}
static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh =
boost::none) {
Q d = g.inverse() * h;
if (Hg)
*Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart
if (Hh)
*Hh = I_3x3; // TODO : check Jacobian consistent with chart (my guess I(3) )
return d;
}
static Q Inverse(const Q &g, ChartJacobian H) {
if (H)
*H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart
return g.inverse();
}
/// Exponential map, simply be converting omega to axis/angle representation
static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
ChartJacobian H = boost::none) {
if (omega.isZero())
return Q::Identity();
else {
_Scalar angle = omega.norm();
return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle));
}
if (H) CONCEPT_NOT_IMPLEMENTED;
}
/// We use our own Logmap, as there is a slight bug in Eigen
static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) {
using std::acos;
using std::sqrt;
// define these compile time constants to avoid std::abs:
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-10;
const double qw = q.w();
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
//return (2 + 2 * (1-qw) / 3) * q.vec();
return (8. / 3. - 2. / 3. * qw) * q.vec();
} else if (qw < NearlyNegativeOne) {
// Taylor expansion of (angle / s) at -1
//return (-2 - 2 * (1 + qw) / 3) * q.vec();
return (-8. / 3 + 2. / 3 * qw) * q.vec();
} else {
// Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
return (angle / s) * q.vec();
}
if (H) CONCEPT_NOT_IMPLEMENTED;
}
/// @}
/// @name Manifold traits
/// @{
static TangentVector Local(const Q& origin, const Q& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
return Logmap(Between(origin, other, Horigin, Hother));
// TODO: incorporate Jacobian of Logmap
}
static Q Retract(const Q& origin, const TangentVector& v,
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
return Compose(origin, Expmap(v), Horigin, Hv);
// TODO : incorporate Jacobian of Expmap
}
/// @}
/// @name Testable
/// @{
static void Print(const Q& q, const std::string& str = "") {
if (str.size() == 0)
std::cout << "Eigen::Quaternion: ";
else
std::cout << str << " ";
std::cout << q.vec().transpose() << std::endl;
}
static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {
return Between(q1, q2).vec().array().abs().maxCoeff() < tol;
}
/// @}
};
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
} // \namespace gtsam

22
gtsam/geometry/Rot2.cpp
View File

@ -17,15 +17,11 @@
*/
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Lie-inl.h>
using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Rot2);
/* ************************************************************************* */
Rot2 Rot2::fromCosSin(double c, double s) {
if (fabs(c * c + s * s - 1.0) > 1e-9) {
@ -63,6 +59,24 @@ Rot2& Rot2::normalize() {
return *this;
}
/* ************************************************************************* */
Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) {
if (H)
*H = I_1x1;
if (zero(v))
return (Rot2());
else
return Rot2::fromAngle(v(0));
}
/* ************************************************************************* */
Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) {
if (H)
*H = I_1x1;
Vector1 v;
v << r.theta();
return v;
}
/* ************************************************************************* */
Matrix2 Rot2::matrix() const {
Matrix2 rvalue_;

113
gtsam/geometry/Rot2.h
View File

@ -18,10 +18,9 @@
#pragma once
#include <boost/optional.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Lie.h>
#include <boost/optional.hpp>
namespace gtsam {
@ -31,25 +30,16 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Rot2 {
public:
/** get the dimension by the type */
static const size_t dimension = 1;
private:
class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
/** we store cos(theta) and sin(theta) */
double c_, s_;
/** normalize to make sure cos and sin form unit vector */
Rot2& normalize();
/** private constructor from cos/sin */
inline Rot2(double c, double s) :
c_(c), s_(s) {
}
inline Rot2(double c, double s) : c_(c), s_(s) {}
public:
@ -57,14 +47,10 @@ namespace gtsam {
/// @{
/** default constructor, zero rotation */
Rot2() :
c_(1.0), s_(0.0) {
}
Rot2() : c_(1.0), s_(0.0) {}
/// Constructor from angle in radians == exponential map at identity
Rot2(double theta) :
c_(cos(theta)), s_(sin(theta)) {
}
Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
/// Named constructor from angle in radians
static Rot2 fromAngle(double theta) {
@ -111,69 +97,25 @@ namespace gtsam {
inline static Rot2 identity() { return Rot2(); }
/** The inverse rotation - negative angle */
Rot2 inverse() const {
return Rot2(c_, -s_);
}
/** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 = I_1x1; // set to 1x1 identity matrix
if (H2) *H2 = I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
Rot2 inverse() const { return Rot2(c_, -s_);}
/** Compose - make a new rotation by adding angles */
Rot2 operator*(const Rot2& R) const {
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
/** Between using the default implementation */
inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 = -I_1x1; // set to 1x1 identity matrix
if (H2) *H2 = I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
}
/// @}
/// @name Manifold
/// @{
/// dimension of the variable - used to autodetect sizes
inline static size_t Dim() {
return dimension;
}
/// Dimensionality of the tangent space, DOF = 1
inline size_t dim() const {
return dimension;
}
/// Updates a with tangent space delta
inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
/// Returns inverse retraction
inline Vector1 localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
/// @}
/// @name Lie Group
/// @{
///Exponential map at identity - create a rotation from canonical coordinates
static Rot2 Expmap(const Vector& v) {
if (zero(v))
return (Rot2());
else
return Rot2::fromAngle(v(0));
}
/// Exponential map at identity - create a rotation from canonical coordinates
static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none);
///Log map at identity - return the canonical coordinates of this rotation
static inline Vector1 Logmap(const Rot2& r) {
Vector1 v;
v << r.theta();
return v;
}
/// Log map at identity - return the canonical coordinates of this rotation
static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
/** Calculate Adjoint map */
Matrix1 AdjointMap() const { return I_1x1; }
/// Left-trivialized derivative of the exponential map
static Matrix ExpmapDerivative(const Vector& v) {
@ -185,6 +127,18 @@ namespace gtsam {
return ones(1);
}
// Chart at origin simply uses exponential map and its inverse
struct ChartAtOrigin {
static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) {
return Expmap(v, H);
}
static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) {
return Logmap(r, H);
}
};
using LieGroup<Rot2, 1>::inverse; // version with derivative
/// @}
/// @name Group Action on Point2
/// @{
@ -253,20 +207,7 @@ namespace gtsam {
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_group<Rot2> : public boost::true_type{
};
struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {};
template<>
struct GTSAM_EXPORT is_manifold<Rot2> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Rot2> : public boost::integral_constant<int, 1>{
};
}
} // gtsam

4
gtsam/geometry/Rot3.cpp
View File

@ -234,8 +234,8 @@ ostream &operator<<(ostream &os, const Rot3& R) {
Rot3 Rot3::slerp(double t, const Rot3& other) const {
// amazingly simple in GTSAM :-)
assert(t>=0 && t<=1);
Vector3 omega = localCoordinates(other, Rot3::EXPMAP);
return retract(t * omega, Rot3::EXPMAP);
Vector3 omega = Logmap(between(other));
return compose(Expmap(t * omega));
}
/* ************************************************************************* */

93
gtsam/geometry/Rot3.h
View File

@ -22,6 +22,9 @@
#pragma once
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
// You can override the default coordinate mode using this flag
@ -40,18 +43,8 @@
#endif
#endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Unit3.h>
namespace gtsam {
/// Typedef to an Eigen Quaternion<double>, we disable alignment because
/// geometry objects are stored in boost pool allocators, in Values
/// containers, and and these pool allocators do not support alignment.
typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
/**
* @brief A 3D rotation represented as a rotation matrix if the preprocessor
* symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it
@ -59,7 +52,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Rot3 {
class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
private:
@ -161,7 +154,7 @@ namespace gtsam {
* @param theta rotation angle
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Vector& w, double theta);
static Rot3 rodriguez(const Vector3& w, double theta);
/**
* Rodriguez' formula to compute an incremental rotation matrix
@ -215,16 +208,13 @@ namespace gtsam {
return Rot3();
}
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/** compose two rotations */
Rot3 operator*(const Rot3& R2) const;
Rot3 inverse() const {
return Rot3(Matrix3(transpose()));
}
/**
* Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C
* @param cRb rotation from B frame to C frame
@ -235,23 +225,10 @@ namespace gtsam {
return cRb * (*this) * cRb.inverse();
}
/**
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R2,
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
/// @}
/// @name Manifold
/// @{
/// dimension of the variable - used to autodetect sizes
static size_t Dim() { return 3; }
/// return dimensionality of tangent space, DOF = 3
size_t dim() const { return 3; }
/**
* The method retract() is used to map from the tangent space back to the manifold.
* Its inverse, is localCoordinates(). For Lie groups, an obvious retraction is the
@ -265,21 +242,29 @@ namespace gtsam {
EXPMAP, ///< Use the Lie group exponential map to retract
#ifndef GTSAM_USE_QUATERNIONS
CAYLEY, ///< Retract and localCoordinates using the Cayley transform.
SLOW_CAYLEY ///< Slow matrix implementation of Cayley transform (for tests only).
#endif
};
#ifndef GTSAM_USE_QUATERNIONS
// Cayley chart around origin
struct CayleyChart {
static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none);
static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none);
};
/// Retraction from R^3 to Rot3 manifold using the Cayley transform
Rot3 retractCayley(const Vector& omega) const;
Rot3 retractCayley(const Vector& omega) const {
return compose(CayleyChart::Retract(omega));
}
/// Inverse of retractCayley
Vector3 localCayley(const Rot3& other) const {
return CayleyChart::Local(between(other));
}
#endif
/// Retraction from R^3 \f$ [R_x,R_y,R_z] \f$ to Rot3 manifold neighborhood around current rotation
Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
/// Returns local retract coordinates \f$ [R_x,R_y,R_z] \f$ in neighborhood around current rotation
Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
/// @}
/// @name Lie Group
/// @{
@ -305,6 +290,17 @@ namespace gtsam {
/// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& x);
/** Calculate Adjoint map */
Matrix3 AdjointMap() const { return matrix(); }
// Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
struct ChartAtOrigin {
static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none);
static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none);
};
using LieGroup<Rot3, 3>::inverse; // version with derivative
/// @}
/// @name Group Action on Point3
/// @{
@ -465,20 +461,7 @@ namespace gtsam {
*/
GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_group<Rot3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT is_manifold<Rot3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Rot3> : public boost::integral_constant<int, 3>{
};
}
struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {};
}

97
gtsam/geometry/Rot3M.cpp
View File

@ -117,7 +117,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
// get components of axis \omega
double wx = w(0), wy=w(1), wz=w(2);
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
@ -139,13 +139,6 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
-swy + C02, swx + C12, c + C22);
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I_3x3;
return *this * R2;
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_));
@ -157,21 +150,6 @@ Matrix3 Rot3::transpose() const {
return rot_.transpose();
}
/* ************************************************************************* */
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -rot_;
return Rot3(Matrix3(transpose()));
}
/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I_3x3;
Matrix3 R12 = transpose()*R2.rot_;
return Rot3(R12);
}
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
@ -228,60 +206,49 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
}
/* ************************************************************************* */
Rot3 Rot3::retractCayley(const Vector& omega) const {
Rot3 Rot3::CayleyChart::Retract(const Vector3& omega, OptionalJacobian<3,3> H) {
if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative");
const double x = omega(0), y = omega(1), z = omega(2);
const double x2 = x * x, y2 = y * y, z2 = z * z;
const double xy = x * y, xz = x * z, yz = y * z;
const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
return (*this)
* Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
(xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
(xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
}
/* ************************************************************************* */
Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) {
return (*this)*Expmap(omega);
} else if(mode == Rot3::CAYLEY) {
return retractCayley(omega);
} else if(mode == Rot3::SLOW_CAYLEY) {
Matrix3 Omega = skewSymmetric(omega);
return (*this)*CayleyFixed<3>(-Omega/2);
} else {
assert(false);
exit(1);
}
Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
// Create a fixed-size matrix
Matrix3 A = R.matrix();
// Mathematica closed form optimization (procrastination?) gone wild:
const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
const double g = A(2, 0), h = A(2, 1), i = A(2, 2);
const double di = d * i, ce = c * e, cd = c * d, fg = f * g;
const double M = 1 + e - f * h + i + e * i;
const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg));
const double x = a * f - cd + f;
const double y = b * f - ce - c;
const double z = fg - di - d;
return K * Vector3(x, y, z);
}
/* ************************************************************************* */
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
if(mode == Rot3::EXPMAP) {
return Logmap(between(T));
} else if(mode == Rot3::CAYLEY) {
// Create a fixed-size matrix
Matrix3 A = rot_.transpose() * T.matrix();
// Mathematica closed form optimization (procrastination?) gone wild:
const double a=A(0,0),b=A(0,1),c=A(0,2);
const double d=A(1,0),e=A(1,1),f=A(1,2);
const double g=A(2,0),h=A(2,1),i=A(2,2);
const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
const double M = 1 + e - f*h + i + e*i;
const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
const double x = a * f - cd + f;
const double y = b * f - ce - c;
const double z = fg - di - d;
return K * Vector3(x, y, z);
} else if(mode == Rot3::SLOW_CAYLEY) {
// Create a fixed-size matrix
Matrix3 A(between(T).matrix());
// using templated version of Cayley
Matrix3 Omega = CayleyFixed<3>(A);
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
} else {
assert(false);
exit(1);
}
Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Expmap(omega, H);
if (mode == Rot3::CAYLEY) return CayleyChart::Retract(omega, H);
else throw std::runtime_error("Rot3::Retract: unknown mode");
}
/* ************************************************************************* */
Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE;
if (mode == Rot3::EXPMAP) return Logmap(R, H);
if (mode == Rot3::CAYLEY) return CayleyChart::Local(R, H);
else throw std::runtime_error("Rot3::Local: unknown mode");
}
/* ************************************************************************* */

37
gtsam/geometry/Rot3Q.cpp
View File

@ -84,8 +84,9 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
return Quaternion(Eigen::AngleAxisd(theta, w)); }
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
return QuaternionChart::Expmap(theta,w);
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
@ -119,7 +120,7 @@ namespace gtsam {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3;
return between_default(*this, R2);
return inverse() * R2;
}
/* ************************************************************************* */
@ -133,35 +134,9 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
using std::acos;
using std::sqrt;
static const double twoPi = 2.0 * M_PI,
// define these compile time constants to avoid std::abs:
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
Vector3 thetaR;
const Quaternion& q = R.quaternion_;
const double qw = q.w();
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
thetaR = (2 - 2 * (qw - 1) / 3) * q.vec();
} else if (qw < NearlyNegativeOne) {
// Angle is zero, return zero vector
thetaR = Vector3::Zero();
} else {
// Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
thetaR = (angle / s) * q.vec();
}
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
if(H) *H = Rot3::LogmapDerivative(thetaR);
return thetaR;
return QuaternionChart::Logmap(R.quaternion_);
}
/* ************************************************************************* */

104
gtsam/geometry/SO3.cpp Normal file
View File

@ -0,0 +1,104 @@
/* ----------------------------------------------------------------------------
* 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 SO3.cpp
* @brief 3*3 matrix representation o SO(3)
* @author Frank Dellaert
* @date December 2014
*/
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/concepts.h>
#include <cmath>
namespace gtsam {
SO3 Rodrigues(const double& theta, const Vector3& axis) {
using std::cos;
using std::sin;
// get components of axis \omega
double wx = axis(0), wy = axis(1), wz = axis(2);
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
double wwTxx = wx * wx, wwTyy = wy * wy, wwTzz = wz * wz;
double swx = wx * s, swy = wy * s, swz = wz * s;
double C00 = c_1 * wwTxx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
double C11 = c_1 * wwTyy, C12 = c_1 * wy * wz;
double C22 = c_1 * wwTzz;
Matrix3 R;
R << c + C00, -swz + C01, swy + C02, //
swz + C01, c + C11, -swx + C12, //
-swy + C02, swx + C12, c + C22;
return R;
}
/// simply convert omega to axis/angle representation
SO3 SO3::Expmap(const Eigen::Ref<const Vector3>& omega,
ChartJacobian H) {
if (H)
CONCEPT_NOT_IMPLEMENTED;
if (omega.isZero())
return SO3::Identity();
else {
double angle = omega.norm();
return Rodrigues(angle, omega / angle);
}
}
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
using std::sqrt;
using std::sin;
if (H)
CONCEPT_NOT_IMPLEMENTED;
// note switch to base 1
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
// Get trace(R)
double tr = R.trace();
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (std::abs(tr + 1.0) < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-10)
return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else {
double magnitude;
double tr_3 = tr - 3.0; // always negative
if (tr_3 < -1e-7) {
double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta));
} else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3 * tr_3 / 12.0;
}
return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
}
}
} // end namespace gtsam

90
gtsam/geometry/SO3.h Normal file
View File

@ -0,0 +1,90 @@
/* ----------------------------------------------------------------------------
* 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 SO3.h
* @brief 3*3 matrix representation of SO(3)
* @author Frank Dellaert
* @date December 2014
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Lie.h>
#include <cmath>
namespace gtsam {
/**
* True SO(3), i.e., 3*3 matrix subgroup
* We guarantee (all but first) constructors only generate from sub-manifold.
* However, round-off errors in repeated composition could move off it...
*/
class SO3: public Matrix3, public LieGroup<SO3,3> {
protected:
public:
enum { dimension=3 };
/// Constructor from AngleAxisd
SO3() : Matrix3(I_3x3) {
}
/// Constructor from Eigen Matrix
template<typename Derived>
SO3(const MatrixBase<Derived>& R) :
Matrix3(R.eval()) {
}
/// Constructor from AngleAxisd
SO3(const Eigen::AngleAxisd& angleAxis) :
Matrix3(angleAxis) {
}
void print(const std::string& s) const {
std::cout << s << *this << std::endl;
}
bool equals(const SO3 & R, double tol) const {
return equal_with_abs_tol(*this, R, tol);
}
static SO3 identity() { return I_3x3; }
SO3 inverse() const { return this->Matrix3::inverse(); }
static SO3 Expmap(const Eigen::Ref<const Vector3>& omega, ChartJacobian H = boost::none);
static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
Matrix3 AdjointMap() const { return *this; }
// Chart at origin
struct ChartAtOrigin {
static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) {
return Expmap(v,H);
}
static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
return Logmap(R,H);
}
};
using LieGroup<SO3,3>::inverse;
};
template<>
struct traits<SO3> : public internal::LieGroupTraits<SO3> {};
} // end namespace gtsam

19
gtsam/geometry/StereoCamera.h
View File

@ -44,6 +44,8 @@ private:
public:
enum { dimension = 6 };
/// @name Standard Constructors
/// @{
@ -144,22 +146,7 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_manifold<StereoCamera> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<StereoCamera> : public boost::integral_constant<int, 6>{
};
template<>
struct GTSAM_EXPORT zero<StereoCamera> {
static StereoCamera value() { return StereoCamera();}
};
}
struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {};
}

18
gtsam/geometry/StereoPoint2.h
View File

@ -121,7 +121,7 @@ namespace gtsam {
/** The difference between another point and this point */
inline StereoPoint2 between(const StereoPoint2& p2) const {
return gtsam::between_default(*this, p2);
return p2 - *this;
}
/// @}
@ -174,20 +174,6 @@ namespace gtsam {
};
// Define GTSAM traits
namespace traits {
template<>
struct is_group<StereoPoint2> : public boost::true_type {
};
template<>
struct is_manifold<StereoPoint2> : public boost::true_type {
};
template<>
struct dimension<StereoPoint2> : public boost::integral_constant<int, 3> {
};
}
struct traits<StereoPoint2> : public internal::Manifold<StereoPoint2> {};
}

5
gtsam/geometry/Unit3.cpp
View File

@ -131,7 +131,7 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
Unit3 Unit3::retract(const Vector2& v) const {
// Get the vector form of the point and the basis matrix
Vector3 p = Point3::Logmap(p_);
Vector3 p = p_.vector();
Matrix32 B = basis();
// Compute the 3D xi_hat vector
@ -156,8 +156,7 @@ Unit3 Unit3::retract(const Vector2& v) const {
/* ************************************************************************* */
Vector2 Unit3::localCoordinates(const Unit3& y) const {
Vector3 p = Point3::Logmap(p_);
Vector3 q = Point3::Logmap(y.p_);
Vector3 p = p_.vector(), q = y.p_.vector();
double dot = p.dot(q);
// Check for special cases

24
gtsam/geometry/Unit3.h
View File

@ -20,6 +20,7 @@
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp>
@ -28,7 +29,7 @@
namespace gtsam {
/// Represents a 3D point on a unit sphere.
class GTSAM_EXPORT Unit3{
class GTSAM_EXPORT Unit3 {
private:
@ -37,6 +38,8 @@ private:
public:
enum { dimension = 2 };
/// @name Constructors
/// @{
@ -160,24 +163,7 @@ private:
};
// Define GTSAM traits
namespace traits {
template<>
struct GTSAM_EXPORT is_manifold<Unit3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Unit3> : public boost::integral_constant<int, 2>{
};
template<>
struct GTSAM_EXPORT zero<Unit3> {
static Unit3 value() {
return Unit3();
}
};
}
template <> struct traits<Unit3> : public internal::Manifold<Unit3> {};
} // namespace gtsam

79
gtsam/geometry/tests/testCyclic.cpp Normal file
View File

@ -0,0 +1,79 @@
/* ----------------------------------------------------------------------------
* 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 testCyclic.cpp
* @brief Unit tests for cyclic group
* @author Frank Dellaert
**/
#include <gtsam/geometry/Cyclic.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
typedef Cyclic<3> G; // Let's use the cyclic group of order 3
//******************************************************************************
TEST(Cyclic, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<G>));
EXPECT_LONGS_EQUAL(0, traits<G>::Identity());
}
//******************************************************************************
TEST(Cyclic, Constructor) {
G g(0);
}
//******************************************************************************
TEST(Cyclic, Compose) {
EXPECT_LONGS_EQUAL(0, traits<G>::Compose(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, traits<G>::Compose(G(0),G(1)));
EXPECT_LONGS_EQUAL(2, traits<G>::Compose(G(0),G(2)));
EXPECT_LONGS_EQUAL(2, traits<G>::Compose(G(2),G(0)));
EXPECT_LONGS_EQUAL(0, traits<G>::Compose(G(2),G(1)));
EXPECT_LONGS_EQUAL(1, traits<G>::Compose(G(2),G(2)));
}
//******************************************************************************
TEST(Cyclic, Between) {
EXPECT_LONGS_EQUAL(0, traits<G>::Between(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, traits<G>::Between(G(0),G(1)));
EXPECT_LONGS_EQUAL(2, traits<G>::Between(G(0),G(2)));
EXPECT_LONGS_EQUAL(1, traits<G>::Between(G(2),G(0)));
EXPECT_LONGS_EQUAL(2, traits<G>::Between(G(2),G(1)));
EXPECT_LONGS_EQUAL(0, traits<G>::Between(G(2),G(2)));
}
//******************************************************************************
TEST(Cyclic, Ivnverse) {
EXPECT_LONGS_EQUAL(0, traits<G>::Inverse(G(0)));
EXPECT_LONGS_EQUAL(2, traits<G>::Inverse(G(1)));
EXPECT_LONGS_EQUAL(1, traits<G>::Inverse(G(2)));
}
//******************************************************************************
TEST(Cyclic , Invariants) {
G g(2), h(1);
check_group_invariants(g,h);
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

42
gtsam/geometry/tests/testPoint2.cpp
View File

@ -27,6 +27,34 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point2)
GTSAM_CONCEPT_LIE_INST(Point2)
//******************************************************************************
TEST(Double , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<double>));
BOOST_CONCEPT_ASSERT((IsManifold<double>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<double>));
}
//******************************************************************************
TEST(Double , Invariants) {
double p1(2), p2(5);
check_group_invariants(p1, p2);
check_manifold_invariants(p1, p2);
}
//******************************************************************************
TEST(Point2 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Point2>));
BOOST_CONCEPT_ASSERT((IsManifold<Point2>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Point2>));
}
//******************************************************************************
TEST(Point2 , Invariants) {
Point2 p1(1, 2), p2(4, 5);
check_group_invariants(p1, p2);
check_manifold_invariants(p1, p2);
}
/* ************************************************************************* */
TEST(Point2, constructor) {
Point2 p1(1, 2), p2 = p1;
@ -38,16 +66,16 @@ TEST(Point2, Lie) {
Point2 p1(1, 2), p2(4, 5);
Matrix H1, H2;
EXPECT(assert_equal(Point2(5,7), p1.compose(p2, H1, H2)));
EXPECT(assert_equal(Point2(5,7), traits<Point2>::Compose(p1, p2, H1, H2)));
EXPECT(assert_equal(eye(2), H1));
EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(Point2(3,3), p1.between(p2, H1, H2)));
EXPECT(assert_equal(Point2(3,3), traits<Point2>::Between(p1, p2, H1, H2)));
EXPECT(assert_equal(-eye(2), H1));
EXPECT(assert_equal(eye(2), H2));
EXPECT(assert_equal(Point2(5,7), p1.retract(Vector2(4., 5.))));
EXPECT(assert_equal(Vector2(3.,3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(Point2(5,7), traits<Point2>::Retract(p1, Vector2(4., 5.))));
EXPECT(assert_equal(Vector2(3.,3.), traits<Point2>::Local(p1,p2)));
}
/* ************************************************************************* */
@ -55,7 +83,7 @@ TEST( Point2, expmap) {
Vector d(2);
d(0) = 1;
d(1) = -1;
Point2 a(4, 5), b = a.retract(d), c(5, 4);
Point2 a(4, 5), b = traits<Point2>::Retract(a,d), c(5, 4);
EXPECT(assert_equal(b,c));
}
@ -108,6 +136,10 @@ TEST( Point2, norm ) {
EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9);
expectedH = numericalDerivative11(norm_proxy, x2);
EXPECT(assert_equal(expectedH,actualH));
// analytical
expectedH = (Matrix(1, 2) << x2.x()/actual, x2.y()/actual).finished();
EXPECT(assert_equal(expectedH,actualH));
}
/* ************************************************************************* */

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

@ -26,22 +26,36 @@ GTSAM_CONCEPT_LIE_INST(Point3)
static Point3 P(0.2, 0.7, -2);
//******************************************************************************
TEST(Point3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Point3>));
BOOST_CONCEPT_ASSERT((IsManifold<Point3>));
BOOST_CONCEPT_ASSERT((IsVectorSpace<Point3>));
}
//******************************************************************************
TEST(Point3 , Invariants) {
Point3 p1(1, 2, 3), p2(4, 5, 6);
check_group_invariants(p1, p2);
check_manifold_invariants(p1, p2);
}
/* ************************************************************************* */
TEST(Point3, Lie) {
Point3 p1(1, 2, 3);
Point3 p2(4, 5, 6);
Matrix H1, H2;
EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2)));
EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Compose(p1, p2, H1, H2)));
EXPECT(assert_equal(eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2)));
EXPECT(assert_equal(Point3(3, 3, 3), traits<Point3>::Between(p1, p2, H1, H2)));
EXPECT(assert_equal(-eye(3), H1));
EXPECT(assert_equal(eye(3), H2));
EXPECT(assert_equal(Point3(5, 7, 9), p1.retract(Vector3(4., 5., 6.))));
EXPECT(assert_equal((Vector)Vector3(3.,3.,3.), p1.localCoordinates(p2)));
EXPECT(assert_equal(Point3(5, 7, 9), traits<Point3>::Retract(p1, Vector3(4,5,6))));
EXPECT(assert_equal(Vector3(3, 3, 3), traits<Point3>::Local(p1,p2)));
}
/* ************************************************************************* */

126
gtsam/geometry/tests/testPose2.cpp
View File

@ -18,7 +18,7 @@
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/lieProxies.h>
#include <CppUnitLite/TestHarness.h>
@ -32,11 +32,16 @@ using namespace boost::assign;
using namespace gtsam;
using namespace std;
//#define SLOW_BUT_CORRECT_EXPMAP
GTSAM_CONCEPT_TESTABLE_INST(Pose2)
GTSAM_CONCEPT_LIE_INST(Pose2)
//******************************************************************************
TEST(Pose2 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Pose2 >));
BOOST_CONCEPT_ASSERT((IsManifold<Pose2 >));
BOOST_CONCEPT_ASSERT((IsLieGroup<Pose2 >));
}
/* ************************************************************************* */
TEST(Pose2, constructors) {
Point2 p;
@ -138,7 +143,6 @@ TEST(Pose2, expmap0d) {
EXPECT(assert_equal(expected, actual, 1e-5));
}
#ifdef SLOW_BUT_CORRECT_EXPMAP
/* ************************************************************************* */
// test case for screw motion in the plane
namespace screw {
@ -149,13 +153,12 @@ namespace screw {
Pose2 expected(expectedR, expectedT);
}
TEST(Pose3, expmap_c)
TEST(Pose2, expmap_c)
{
EXPECT(assert_equal(screw::expected, expm<Pose2>(screw::xi),1e-6));
EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6));
EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6));
}
#endif
/* ************************************************************************* */
TEST(Pose2, expmap_c_full)
@ -175,9 +178,9 @@ TEST(Pose2, logmap) {
Pose2 pose0(M_PI/2.0, Point2(1, 2));
Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01));
#ifdef SLOW_BUT_CORRECT_EXPMAP
Vector expected = Vector3(0.00986473, -0.0150896, 0.018);
Vector3 expected(0.00986473, -0.0150896, 0.018);
#else
Vector expected = Vector3(0.01, -0.015, 0.018);
Vector3 expected(0.01, -0.015, 0.018);
#endif
Vector actual = pose0.localCoordinates(pose);
EXPECT(assert_equal(expected, actual, 1e-5));
@ -193,34 +196,45 @@ TEST(Pose2, logmap_full) {
}
/* ************************************************************************* */
Vector3 w = (Vector(3) << 0.1, 0.27, -0.3).finished();
Vector3 w0 = (Vector(3) << 0.1, 0.27, 0.0).finished(); // alpha = 0
// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector3 testDexpL(const Vector3 w, const Vector3& dw) {
Vector3 y = Pose2::Logmap(Pose2::Expmap(-w) * Pose2::Expmap(w + dw));
return y;
TEST( Pose2, ExpmapDerivative1) {
Matrix3 actualH;
Vector3 w(0.1, 0.27, -0.3);
Pose2::Expmap(w,actualH);
Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
OptionalJacobian<3, 3> >(&Pose2::Expmap, w, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
TEST( Pose2, ExpmapDerivative) {
Matrix actualDexpL = Pose2::ExpmapDerivative(w);
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(
boost::bind(testDexpL, w, _1), zero(3), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
/* ************************************************************************* */
TEST( Pose2, ExpmapDerivative2) {
Matrix3 actualH;
Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
Pose2::Expmap(w0,actualH);
Matrix3 expectedH = numericalDerivative21<Pose2, Vector3,
OptionalJacobian<3, 3> >(&Pose2::Expmap, w0, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
Matrix actualDexpInvL = Pose2::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
/* ************************************************************************* */
TEST( Pose2, LogmapDerivative1) {
Matrix3 actualH;
Vector3 w(0.1, 0.27, -0.3);
Pose2 p = Pose2::Expmap(w);
EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5));
Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
OptionalJacobian<3, 3> >(&Pose2::Logmap, p, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
// test case where alpha = 0
Matrix actualDexpL0 = Pose2::ExpmapDerivative(w0);
Matrix expectedDexpL0 = numericalDerivative11<Vector3, Vector3>(
boost::bind(testDexpL, w0, _1), zero(3), 1e-2);
EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5));
Matrix actualDexpInvL0 = Pose2::LogmapDerivative(w0);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
/* ************************************************************************* */
TEST( Pose2, LogmapDerivative2) {
Matrix3 actualH;
Vector3 w0(0.1, 0.27, 0.0); // alpha = 0
Pose2 p = Pose2::Expmap(w0);
EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5));
Matrix3 expectedH = numericalDerivative21<Vector3, Pose2,
OptionalJacobian<3, 3> >(&Pose2::Logmap, p, boost::none, 1e-2);
EXPECT(assert_equal(expectedH, actualH, 1e-5));
}
/* ************************************************************************* */
@ -513,13 +527,6 @@ TEST( Pose2, round_trip )
EXPECT(assert_equal(odo, p1.between(p2)));
}
/* ************************************************************************* */
TEST(Pose2, members)
{
Pose2 pose;
EXPECT(pose.dim() == 3);
}
namespace {
/* ************************************************************************* */
// some shared test values
@ -760,6 +767,47 @@ TEST(Pose2, align_4) {
EXPECT(assert_equal(expected, *actual));
}
//******************************************************************************
Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5)));
Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
//******************************************************************************
TEST(Pose2 , Invariants) {
Pose2 id;
check_group_invariants(id,id);
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
}
//******************************************************************************
TEST(Pose2 , LieGroupDerivatives) {
Pose2 id;
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
}
//******************************************************************************
TEST(Pose2 , ChartDerivatives) {
Pose2 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -16,9 +16,8 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/assign/std/vector.hpp> // for operator +=
using namespace boost::assign;
@ -57,25 +56,24 @@ TEST( Pose3, constructors)
}
/* ************************************************************************* */
#ifndef GTSAM_POSE3_EXPMAP
TEST( Pose3, retract_first_order)
{
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::FIRST_ORDER),1e-2));
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2));
v(3)=0.2;v(4)=0.7;v(5)=-2;
EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::FIRST_ORDER),1e-2));
EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2));
}
#endif
/* ************************************************************************* */
TEST( Pose3, retract_expmap)
{
Pose3 id;
Vector v = zero(6);
v(0) = 0.3;
EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::EXPMAP),1e-2));
v(3)=0.2;v(4)=0.394742;v(5)=-2.08998;
EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::EXPMAP),1e-2));
Vector v = zero(6); v(0) = 0.3;
Pose3 pose = Pose3::Expmap(v);
EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2));
EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2));
}
/* ************************************************************************* */
@ -151,11 +149,11 @@ Pose3 Agrawal06iros(const Vector& xi) {
Vector v = xi.tail(3);
double t = norm_2(w);
if (t < 1e-5)
return Pose3(Rot3(), Point3::Expmap(v));
return Pose3(Rot3(), Point3(v));
else {
Matrix W = skewSymmetric(w/t);
Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W);
return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v));
return Pose3(Rot3::Expmap (w), Point3(A * v));
}
}
@ -463,43 +461,51 @@ TEST( Pose3, transformPose_to)
}
/* ************************************************************************* */
TEST(Pose3, localCoordinates_first_order)
TEST(Pose3, Retract_LocalCoordinates)
{
Vector6 d;
d << 1,2,3,4,5,6; d/=10;
R = Rot3::Retract(d.head<3>());
Pose3 t = Pose3::Retract(d);
EXPECT(assert_equal(d, Pose3::LocalCoordinates(t)));
}
/* ************************************************************************* */
TEST(Pose3, retract_localCoordinates)
{
Vector6 d12;
d12 << 1,2,3,4,5,6; d12/=10;
Pose3 t1 = T, t2 = t1.retract(d12);
EXPECT(assert_equal(d12, t1.localCoordinates(t2)));
}
/* ************************************************************************* */
TEST(Pose3, expmap_logmap)
{
Vector d12 = repeat(6,0.1);
Pose3 t1 = T, t2 = t1.retract(d12, Pose3::FIRST_ORDER);
EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::FIRST_ORDER)));
Pose3 t1 = T, t2 = t1.expmap(d12);
EXPECT(assert_equal(d12, t1.logmap(t2)));
}
/* ************************************************************************* */
TEST(Pose3, localCoordinates_expmap)
{
Vector d12 = repeat(6,0.1);
Pose3 t1 = T, t2 = t1.retract(d12, Pose3::EXPMAP);
EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::EXPMAP)));
}
/* ************************************************************************* */
TEST(Pose3, manifold_first_order)
TEST(Pose3, retract_localCoordinates2)
{
Pose3 t1 = T;
Pose3 t2 = T3;
Pose3 origin;
Vector d12 = t1.localCoordinates(t2, Pose3::FIRST_ORDER);
EXPECT(assert_equal(t2, t1.retract(d12, Pose3::FIRST_ORDER)));
Vector d21 = t2.localCoordinates(t1, Pose3::FIRST_ORDER);
EXPECT(assert_equal(t1, t2.retract(d21, Pose3::FIRST_ORDER)));
Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
}
/* ************************************************************************* */
TEST(Pose3, manifold_expmap)
{
Pose3 t1 = T;
Pose3 t2 = T3;
Pose3 origin;
Vector d12 = t1.localCoordinates(t2, Pose3::EXPMAP);
EXPECT(assert_equal(t2, t1.retract(d12, Pose3::EXPMAP)));
Vector d21 = t2.localCoordinates(t1, Pose3::EXPMAP);
EXPECT(assert_equal(t1, t2.retract(d21, Pose3::EXPMAP)));
Vector d12 = t1.logmap(t2);
EXPECT(assert_equal(t2, t1.expmap(d12)));
Vector d21 = t2.logmap(t1);
EXPECT(assert_equal(t1, t2.expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)
EXPECT(assert_equal(d12,-d21));
@ -672,26 +678,28 @@ TEST(Pose3, align_2) {
}
/* ************************************************************************* */
/// exp(xi) exp(y) = exp(xi + dxi)
/// Hence, y = log (exp(-xi)*exp(xi+dxi))
Vector6 xi = (Vector(6) << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0).finished();
Vector6 testExpmapDerivative(const Vector6& xi, const Vector6& dxi) {
return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi));
}
TEST( Pose3, ExpmapDerivative) {
Matrix actualDexpL = Pose3::ExpmapDerivative(xi);
Matrix expectedDexpL = numericalDerivative11<Vector6, Vector6>(
boost::bind(testExpmapDerivative, xi, _1), zero(6), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Pose3::LogmapDerivative(xi);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
TEST( Pose3, ExpmapDerivative1) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3::Expmap(w,actualH);
Matrix expectedH = numericalDerivative21<Pose3, Vector6,
OptionalJacobian<6, 6> >(&Pose3::Expmap, w, boost::none);
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) {
TEST( Pose3, LogmapDerivative1) {
Matrix6 actualH;
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
Pose3 p = Pose3::Expmap(w);
EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5));
Matrix expectedH = numericalDerivative21<Vector6, Pose3,
OptionalJacobian<6, 6> >(&Pose3::Logmap, p, boost::none);
EXPECT(assert_equal(expectedH, actualH));
}
/* ************************************************************************* */
Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi) * v;
}
@ -701,7 +709,7 @@ TEST( Pose3, adjoint) {
Matrix actualH;
Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH);
Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>(
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjoint, screw::xi, screw::xi, 1e-5);
EXPECT(assert_equal(expected,actual,1e-5));
@ -709,7 +717,7 @@ TEST( Pose3, adjoint) {
}
/* ************************************************************************* */
Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) {
Vector6 testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi).transpose() * v;
}
@ -721,7 +729,7 @@ TEST( Pose3, adjointTranspose) {
Matrix actualH;
Vector actual = Pose3::adjointTranspose(xi, v, actualH);
Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>(
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjointTranspose, xi, v, 1e-5);
EXPECT(assert_equal(expected,actual,1e-15));
@ -737,6 +745,43 @@ TEST( Pose3, stream)
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n");
}
//******************************************************************************
TEST(Pose3 , Invariants) {
Pose3 id;
check_group_invariants(id,id);
check_group_invariants(id,T3);
check_group_invariants(T2,id);
check_group_invariants(T2,T3);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T3);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T3);
}
//******************************************************************************
TEST(Pose3 , LieGroupDerivatives) {
Pose3 id;
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T3);
}
//******************************************************************************
TEST(Pose3 , ChartDerivatives) {
Pose3 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T3);
}
/* ************************************************************************* */
int main(){ TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

126
gtsam/geometry/tests/testQuaternion.cpp Normal file
View File

@ -0,0 +1,126 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testQuaternion.cpp
* @brief Unit tests for Quaternion, as a GTSAM-adapted Lie Group
* @author Frank Dellaert
**/
#include <gtsam/geometry/Quaternion.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
typedef Quaternion Q; // Typedef
typedef traits<Q>::ChartJacobian QuaternionJacobian;
//******************************************************************************
TEST(Quaternion , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
}
//******************************************************************************
TEST(Quaternion , Constructor) {
Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
}
//******************************************************************************
TEST(Quaternion , Invariants) {
Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0)));
check_group_invariants(q1, q2);
check_manifold_invariants(q1, q2);
}
//******************************************************************************
TEST(Quaternion , Local) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0, z_axis));
Q q2(Eigen::AngleAxisd(0.1, z_axis));
QuaternionJacobian H1, H2;
Vector3 expected(0, 0, 0.1);
Vector3 actual = traits<Q>::Local(q1, q2, H1, H2);
EXPECT(assert_equal((Vector)expected,actual));
}
//******************************************************************************
TEST(Quaternion , Retract) {
Vector3 z_axis(0, 0, 1);
Q q(Eigen::AngleAxisd(0, z_axis));
Q expected(Eigen::AngleAxisd(0.1, z_axis));
Vector3 v(0, 0, 0.1);
QuaternionJacobian Hq, Hv;
Q actual = traits<Q>::Retract(q, v, Hq, Hv);
EXPECT(actual.isApprox(expected));
}
//******************************************************************************
TEST(Quaternion , Compose) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.2, z_axis));
Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1 * q2;
Matrix actualH1, actualH2;
Q actual = traits<Q>::Compose(q1, q2, actualH1, actualH2);
EXPECT(traits<Q>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<Q>::Compose, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<Q>::Compose, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
}
//******************************************************************************
TEST(Quaternion , Between) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.2, z_axis));
Q q2(Eigen::AngleAxisd(0.1, z_axis));
Q expected = q1.inverse() * q2;
Matrix actualH1, actualH2;
Q actual = traits<Q>::Between(q1, q2, actualH1, actualH2);
EXPECT(traits<Q>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<Q>::Between, q1, q2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<Q>::Between, q1, q2);
EXPECT(assert_equal(numericalH2,actualH2));
}
//******************************************************************************
TEST(Quaternion , Inverse) {
Vector3 z_axis(0, 0, 1);
Q q1(Eigen::AngleAxisd(0.1, z_axis));
Q expected(Eigen::AngleAxisd(-0.1, z_axis));
Matrix actualH;
Q actual = traits<Q>::Inverse(q1, actualH);
EXPECT(traits<Q>::Equals(expected,actual));
Matrix numericalH = numericalDerivative11(traits<Q>::Inverse, q1);
EXPECT(assert_equal(numericalH,actualH));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

57
gtsam/geometry/tests/testRot2.cpp
View File

@ -15,10 +15,10 @@
* @author Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
@ -155,26 +155,45 @@ TEST( Rot2, relativeBearing )
CHECK(assert_equal(expectedH,actualH));
}
/* ************************************************************************* */
Vector w = (Vector(1) << 0.27).finished();
//******************************************************************************
Rot2 T1(0.1);
Rot2 T2(0.2);
//******************************************************************************
TEST(Rot2 , Invariants) {
Rot2 id;
check_group_invariants(id,id);
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector1 testDexpL(const Vector& dw) {
Vector1 y = Rot2::Logmap(Rot2::Expmap(-w) * Rot2::Expmap(w + dw));
return y;
}
TEST( Rot2, ExpmapDerivative) {
Matrix actualDexpL = Rot2::ExpmapDerivative(w);
Matrix expectedDexpL = numericalDerivative11<Vector, Vector1>(
boost::function<Vector(const Vector&)>(
boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
//******************************************************************************
TEST(Rot2 , LieGroupDerivatives) {
Rot2 id;
Matrix actualDexpInvL = Rot2::LogmapDerivative(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
}
//******************************************************************************
TEST(Rot2 , ChartDerivatives) {
Rot2 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
}
/* ************************************************************************* */

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

@ -18,11 +18,10 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/chartTesting.h>
#include <boost/math/constants/constants.hpp>
@ -38,12 +37,18 @@ static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001;
//******************************************************************************
TEST(Rot3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Rot3 >));
BOOST_CONCEPT_ASSERT((IsManifold<Rot3 >));
BOOST_CONCEPT_ASSERT((IsLieGroup<Rot3 >));
}
/* ************************************************************************* */
TEST( Rot3, chart)
{
Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished();
Rot3 rot3(R);
CHECK_CHART_CONCEPT(rot3);
}
/* ************************************************************************* */
@ -148,10 +153,10 @@ TEST( Rot3, retract)
Vector v = zero(3);
CHECK(assert_equal(R, R.retract(v)));
// test Canonical coordinates
Canonical<Rot3> chart;
Vector v2 = chart.local(R);
CHECK(assert_equal(R, chart.retract(v2)));
// // test Canonical coordinates
// Canonical<Rot3> chart;
// Vector v2 = chart.local(R);
// CHECK(assert_equal(R, chart.retract(v2)));
}
/* ************************************************************************* */
@ -214,6 +219,30 @@ TEST(Rot3, log)
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
}
/* ************************************************************************* */
TEST(Rot3, retract_localCoordinates)
{
Vector3 d12 = repeat(3,0.1);
Rot3 R2 = R.retract(d12);
EXPECT(assert_equal(d12, R.localCoordinates(R2)));
}
/* ************************************************************************* */
TEST(Rot3, expmap_logmap)
{
Vector3 d12 = repeat(3,0.1);
Rot3 R2 = R.expmap(d12);
EXPECT(assert_equal(d12, R.logmap(R2)));
}
/* ************************************************************************* */
TEST(Rot3, retract_localCoordinates2)
{
Rot3 t1 = R, t2 = R*R, origin;
Vector d12 = t1.localCoordinates(t2);
EXPECT(assert_equal(t2, t1.retract(d12)));
Vector d21 = t2.localCoordinates(t1);
EXPECT(assert_equal(t1, t2.retract(d21)));
}
/* ************************************************************************* */
Vector w = Vector3(0.1, 0.27, -0.2);
@ -288,12 +317,10 @@ TEST(Rot3, manifold_expmap)
Rot3 origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP)));
Vector d12 = Rot3::Logmap(gR1.between(gR2));
Vector d21 = Rot3::Logmap(gR2.between(gR1));
// Check that it is expmap
// Check expmap
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
@ -590,6 +617,12 @@ TEST(Rot3, quaternion) {
}
/* ************************************************************************* */
Matrix Cayley(const Matrix& A) {
Matrix::Index n = A.cols();
const Matrix I = eye(n);
return (I-A)*inverse(I+A);
}
TEST( Rot3, Cayley ) {
Matrix A = skewSymmetric(1,2,-3);
Matrix Q = Cayley(A);
@ -618,6 +651,47 @@ TEST( Rot3, slerp)
EXPECT(assert_equal(R1, R1.slerp(0.5,R1)));
}
//******************************************************************************
Rot3 T1(Rot3::rodriguez(Vector3(0, 0, 1), 1));
Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
//******************************************************************************
TEST(Rot3 , Invariants) {
Rot3 id;
check_group_invariants(id,id);
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
}
//******************************************************************************
TEST(Rot3 , LieGroupDerivatives) {
Rot3 id;
CHECK_LIE_GROUP_DERIVATIVES(id,id);
CHECK_LIE_GROUP_DERIVATIVES(id,T2);
CHECK_LIE_GROUP_DERIVATIVES(T2,id);
CHECK_LIE_GROUP_DERIVATIVES(T2,T1);
}
//******************************************************************************
TEST(Rot3 , ChartDerivatives) {
Rot3 id;
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,T2);
CHECK_CHART_DERIVATIVES(T2,id);
CHECK_CHART_DERIVATIVES(T2,T1);
}
/* ************************************************************************* */
int main() {
TestResult tr;

47
gtsam/geometry/tests/testRot3M.cpp
View File

@ -10,21 +10,14 @@
* -------------------------------------------------------------------------- */
/**
* @file testRot3.cpp
* @brief Unit tests for Rot3 class
* @file testRot3M.cpp
* @brief Unit tests for Rot3 class, matrix version
* @author Alireza Fathi
* @author Frank Dellaert
*/
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <boost/math/constants/constants.hpp>
#include <CppUnitLite/TestHarness.h>
#ifndef GTSAM_USE_QUATERNIONS
@ -46,38 +39,10 @@ TEST(Rot3, manifold_cayley)
Rot3 origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::CAYLEY);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CAYLEY)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::CAYLEY);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CAYLEY)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = Vector3(0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Rot3 R2 = Rot3::Expmap (2 * d);
Rot3 R3 = Rot3::Expmap (3 * d);
Rot3 R5 = Rot3::Expmap (5 * d);
CHECK(assert_equal(R5,R2*R3));
CHECK(assert_equal(R5,R3*R2));
}
/* ************************************************************************* */
TEST(Rot3, manifold_slow_cayley)
{
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::SLOW_CAYLEY);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CAYLEY)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CAYLEY);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CAYLEY)));
Vector d12 = gR1.localCayley(gR2);
CHECK(assert_equal(gR2, gR1.retractCayley(d12)));
Vector d21 = gR2.localCayley(gR1);
CHECK(assert_equal(gR1, gR2.retractCayley(d21)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));

134
gtsam/geometry/tests/testSO3.cpp Normal file
View File

@ -0,0 +1,134 @@
/* ----------------------------------------------------------------------------
* 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 testQuaternion.cpp
* @brief Unit tests for SO3, as a GTSAM-adapted Lie Group
* @author Frank Dellaert
**/
#include <gtsam/geometry/SO3.h>
#include <gtsam/base/testLie.h>
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
TEST(SO3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
BOOST_CONCEPT_ASSERT((IsManifold<SO3 >));
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3 >));
}
//******************************************************************************
TEST(SO3 , Constructor) {
SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
}
//******************************************************************************
SO3 id;
Vector3 z_axis(0, 0, 1);
SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
//******************************************************************************
TEST(SO3 , Local) {
Vector3 expected(0, 0, 0.1);
Vector3 actual = traits<SO3>::Local(R1, R2);
EXPECT(assert_equal((Vector)expected,actual));
}
//******************************************************************************
TEST(SO3 , Retract) {
Vector3 v(0, 0, 0.1);
SO3 actual = traits<SO3>::Retract(R1, v);
EXPECT(actual.isApprox(R2));
}
//******************************************************************************
TEST(SO3 , Compose) {
SO3 expected = R1 * R2;
Matrix actualH1, actualH2;
SO3 actual = traits<SO3>::Compose(R1, R2, actualH1, actualH2);
EXPECT(traits<SO3>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<SO3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<SO3>::Compose, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2));
}
//******************************************************************************
TEST(SO3 , Between) {
SO3 expected = R1.inverse() * R2;
Matrix actualH1, actualH2;
SO3 actual = traits<SO3>::Between(R1, R2, actualH1, actualH2);
EXPECT(traits<SO3>::Equals(expected,actual));
Matrix numericalH1 = numericalDerivative21(traits<SO3>::Between, R1, R2);
EXPECT(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(traits<SO3>::Between, R1, R2);
EXPECT(assert_equal(numericalH2,actualH2));
}
//******************************************************************************
TEST(SO3 , Inverse) {
SO3 expected(Eigen::AngleAxisd(-0.1, z_axis));
Matrix actualH;
SO3 actual = traits<SO3>::Inverse(R1, actualH);
EXPECT(traits<SO3>::Equals(expected,actual));
Matrix numericalH = numericalDerivative11(traits<SO3>::Inverse, R1);
EXPECT(assert_equal(numericalH,actualH));
}
//******************************************************************************
TEST(SO3 , Invariants) {
check_group_invariants(id,id);
check_group_invariants(id,R1);
check_group_invariants(R2,id);
check_group_invariants(R2,R1);
check_manifold_invariants(id,id);
check_manifold_invariants(id,R1);
check_manifold_invariants(R2,id);
check_manifold_invariants(R2,R1);
}
//******************************************************************************
//TEST(SO3 , LieGroupDerivatives) {
// CHECK_LIE_GROUP_DERIVATIVES(id,id);
// CHECK_LIE_GROUP_DERIVATIVES(id,R2);
// CHECK_LIE_GROUP_DERIVATIVES(R2,id);
// CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
//}
//******************************************************************************
TEST(SO3 , ChartDerivatives) {
CHECK_CHART_DERIVATIVES(id,id);
CHECK_CHART_DERIVATIVES(id,R2);
CHECK_CHART_DERIVATIVES(R2,id);
CHECK_CHART_DERIVATIVES(R2,R1);
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

9
gtsam/geometry/tests/testStereoPoint2.cpp
View File

@ -26,9 +26,14 @@ using namespace std;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2)
GTSAM_CONCEPT_LIE_INST(StereoPoint2)
//GTSAM_CONCEPT_LIE_INST(StereoPoint2)
//******************************************************************************
TEST(StereoPoint2 , Concept) {
BOOST_CONCEPT_ASSERT((IsManifold<StereoPoint2 >));
}
/* ************************************************************************* */
TEST(StereoPoint2, constructor) {
StereoPoint2 p1(1, 2, 3), p2 = p1;
@ -49,7 +54,7 @@ TEST(StereoPoint2, Lie) {
}
/* ************************************************************************* */
TEST( StereoPoint2, expmap) {
TEST( StereoPoint2, retract) {
Vector d(3);
d(0) = 1;
d(1) = -1;

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

@ -115,13 +115,13 @@ TEST(Unit3, error) {
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Vector,Unit3>(
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q);
p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
}
{
expected = numericalDerivative11<Vector,Unit3>(
expected = numericalDerivative11<Vector2,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r);
p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));

1
gtsam/inference/FactorGraph.h
View File

@ -250,7 +250,6 @@ namespace gtsam {
void print(const std::string& s = "FactorGraph",
const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected:
/** Check equality */
bool equals(const This& fg, double tol = 1e-9) const;
/// @}

3
gtsam/inference/LabeledSymbol.h
View File

@ -130,5 +130,8 @@ inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label();
/** Return the index portion of a symbol key. */
inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); }
/// traits
template<> struct traits<LabeledSymbol> : public Testable<LabeledSymbol> {};
} // \namespace gtsam

4
gtsam/inference/Ordering.h
View File

@ -191,5 +191,9 @@ namespace gtsam {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
/// traits
template<> struct traits<Ordering> : public Testable<Ordering> {};
}

9
gtsam/inference/Symbol.h
View File

@ -18,9 +18,9 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <gtsam/inference/Key.h>
#include <gtsam/base/Testable.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
@ -161,5 +161,8 @@ inline Key Y(size_t j) { return Symbol('y', j); }
inline Key Z(size_t j) { return Symbol('z', j); }
}
} // namespace gtsam
/// traits
template<> struct traits<Symbol> : public Testable<Symbol> {};
} // \ namespace gtsam

23
gtsam/inference/VariableIndex.h
View File

@ -17,16 +17,18 @@
#pragma once
#include <gtsam/inference/Key.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/types.h>
#include <gtsam/base/timing.h>
#include <boost/foreach.hpp>
#include <vector>
#include <deque>
#include <stdexcept>
#include <boost/foreach.hpp>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/types.h>
#include <gtsam/base/timing.h>
#include <gtsam/inference/Key.h>
namespace gtsam {
@ -175,6 +177,11 @@ protected:
/// @}
};
}
/// traits
template<>
struct traits<VariableIndex> : public Testable<VariableIndex> {
};
} //\ namespace gtsam
#include <gtsam/inference/VariableIndex-inl.h>

7
gtsam/inference/VariableSlots.h
View File

@ -22,12 +22,12 @@
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/timing.h>
#include <iostream>
#include <gtsam/base/Testable.h>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <iostream>
#include <string>
namespace gtsam {
@ -82,6 +82,9 @@ public:
/// @}
};
/// traits
template<> struct traits<VariableSlots> : public Testable<VariableSlots> {};
/* ************************************************************************* */
template<class FG>
VariableSlots::VariableSlots(const FG& factorGraph)

2
gtsam/linear/ConjugateGradientSolver.h
View File

@ -149,6 +149,8 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju
<< ", alpha = " << alpha
<< ", beta = " << beta
<< ", ||r||^2 = " << currentGamma
// << "\nx =\n" << estimate
// << "\nr =\n" << residual
<< std::endl;
}
if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )

8
gtsam/linear/Errors.h
View File

@ -21,6 +21,7 @@
#include <gtsam/base/FastList.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Testable.h>
#include <string>
@ -70,4 +71,9 @@ namespace gtsam {
/** print with optional string */
GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error");
} // gtsam
/// traits
template<>
struct traits<Errors> : public Testable<Errors> {
};
} //\ namespace gtsam

7
gtsam/linear/GaussianBayesNet.h
View File

@ -171,4 +171,9 @@ namespace gtsam {
}
};
} /// namespace gtsam
/// traits
template<>
struct traits<GaussianBayesNet> : public Testable<GaussianBayesNet> {
};
} //\ namespace gtsam

7
gtsam/linear/GaussianBayesTree.h
View File

@ -128,4 +128,9 @@ namespace gtsam {
Matrix marginalCovariance(Key key) const;
};
}
/// traits
template<>
struct traits<GaussianBayesTree> : public Testable<GaussianBayesTree> {
};
} //\ namespace gtsam

6
gtsam/linear/GaussianConditional.h
View File

@ -141,7 +141,11 @@ namespace gtsam {
}
}; // GaussianConditional
} // gtsam
/// traits
template<>
struct traits<GaussianConditional> : public Testable<GaussianConditional> {};
} // \ namespace gtsam
#include <gtsam/linear/GaussianConditional-inl.h>

10
gtsam/linear/GaussianFactor.h
View File

@ -20,8 +20,9 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
namespace gtsam {
@ -146,4 +147,9 @@ namespace gtsam {
}; // GaussianFactor
} // namespace gtsam
/// traits
template<>
struct traits<GaussianFactor> : public Testable<GaussianFactor> {
};
} // \ namespace gtsam

7
gtsam/linear/GaussianFactorGraph.h
View File

@ -344,4 +344,9 @@ namespace gtsam {
//GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
//GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
} // namespace gtsam
/// traits
template<>
struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
};
} // \ namespace gtsam

7
gtsam/linear/HessianFactor.h
View File

@ -443,6 +443,11 @@ namespace gtsam {
}
};
}
/// traits
template<>
struct traits<HessianFactor> : public Testable<HessianFactor> {};
} // \ namespace gtsam
#include <gtsam/linear/HessianFactor-inl.h>

7
gtsam/linear/JacobianFactor.h
View File

@ -360,7 +360,12 @@ namespace gtsam {
}
}; // JacobianFactor
} // gtsam
/// traits
template<>
struct traits<JacobianFactor> : public Testable<JacobianFactor> {
};
} // \ namespace gtsam
#include <gtsam/linear/JacobianFactor-inl.h>

13
gtsam/linear/NoiseModel.h
View File

@ -18,12 +18,14 @@
#pragma once
#include <gtsam/base/Testable.h>
#include <gtsam/base/Matrix.h>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/extended_type_info.hpp>
#include <boost/serialization/singleton.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <cmath>
namespace gtsam {
@ -885,6 +887,13 @@ namespace gtsam {
typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
typedef noiseModel::Constrained::shared_ptr SharedConstrained;
} // namespace gtsam
/// traits
template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
template<> struct traits<noiseModel::Diagonal> : public Testable<noiseModel::Diagonal> {};
template<> struct traits<noiseModel::Constrained> : public Testable<noiseModel::Constrained> {};
template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
} //\ namespace gtsam

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