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

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

Partial update of Similarity to BAD

Conflicts:
	.gitignore
release/4.3a0
Paul Drews 2015-01-23 08:10:21 -05:00
parent bade68fa33 47811e3a64
commit d28ef19b9a
323 changed files with 11966 additions and 7046 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

224
.cproject
View File

@ -510,22 +510,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SFMExampleExpressions.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>SFMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExampleExpressions.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -542,6 +526,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEvent.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testEvent.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
@ -856,6 +848,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSmartStereoProjectionPoseFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSmartStereoProjectionPoseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTOAFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testTOAFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -960,38 +968,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeCameraExpression.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeCameraExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeOneCameraExpression.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeOneCameraExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeSFMExpressions.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeSFMExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeAdaptAutoDiff.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCombinedImuFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1583,6 +1559,14 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLabeledSymbol.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testLabeledSymbol.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1994,6 +1978,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.navigation" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2 VERBOSE=1</buildArguments>
<buildTarget>check.navigation</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2177,6 +2169,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>
@ -2537,6 +2537,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testOptionalJacobian.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testOptionalJacobian.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2766,14 +2774,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpressionFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testExpressionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpressionMeta.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2782,30 +2782,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAdaptAutoDiff.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCallRecord.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCallRecord.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBasisDecompositions.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testBasisDecompositions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
@ -2950,6 +2926,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRangeFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testRangeFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3126,6 +3110,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExampleExpressions.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>Pose2SLAMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SFMExampleExpressions.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>SFMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -3174,6 +3174,30 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAdaptAutoDiff.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCallRecord.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testCallRecord.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpressionFactor.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testExpressionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
@ -3222,6 +3246,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeAdaptAutoDiff.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeCameraExpression.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeCameraExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeOneCameraExpression.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeOneCameraExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeSFMExpressions.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeSFMExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeIncremental.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeIncremental.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

4
.gitignore vendored
View File

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

1
.settings/.gitignore vendored Normal file
View File

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

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

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

6
CppUnitLite/Test.h
View File

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

206
GTSAM-Concepts.md Normal file
View File

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

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

2
cmake/GtsamBuildTypes.cmake
View File

@ -54,7 +54,7 @@ if(NOT FIRST_PASS_DONE)
endif()
# Clang on Mac uses a template depth that is less than standard and is too small
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
endif()

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

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

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

@ -18,8 +18,8 @@
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam_unstable/slam/expressions.h>
#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -35,26 +35,26 @@ using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
ExpressionFactorGraph graph;
// Create Expressions for unknowns
Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(ExpressionFactor<Pose2>(priorNoise, Pose2(0, 0, 0), x1));
graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, 0 ), between(x1,x2)));
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x2,x3)));
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x3,x4)));
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x4,x5)));
graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model);
graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
graph.push_back(ExpressionFactor<Pose2>(model, Pose2(2, 0, M_PI_2), between(x5,x2)));
graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution

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

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

36
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)
@ -295,7 +295,7 @@ void runIncremental()
NonlinearFactorGraph newFactors;
Values newVariables;
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(Pose::Dim())));
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(3)));
newVariables.insert(firstPose, Pose());
isam2.update(newFactors, newVariables);
@ -474,7 +474,7 @@ void runBatch()
cout << "Creating batch optimizer..." << endl;
NonlinearFactorGraph measurements = datasetMeasurements;
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(3)));
gttic_(Create_optimizer);
GaussNewtonParams params;

182
gtsam.h
View File

@ -156,12 +156,6 @@ virtual class Value {
size_t dim() const;
};
class Vector3 {
Vector3(Vector v);
};
class Vector6 {
Vector6(Vector v);
};
#include <gtsam/base/LieScalar.h>
class LieScalar {
// Standard constructors
@ -276,8 +270,6 @@ class Point2 {
gtsam::Point2 between(const gtsam::Point2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Point2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point2& p) const;
@ -348,8 +340,6 @@ class Point3 {
gtsam::Point3 between(const gtsam::Point3& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Point3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point3& p) const;
@ -386,8 +376,6 @@ class Rot2 {
gtsam::Rot2 between(const gtsam::Rot2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Rot2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot2& p) const;
@ -439,8 +427,6 @@ class Rot3 {
gtsam::Rot3 between(const gtsam::Rot3& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
//gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const;
@ -488,8 +474,6 @@ class Pose2 {
gtsam::Pose2 between(const gtsam::Pose2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Pose2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Pose2& p) const;
@ -537,10 +521,7 @@ class Pose3 {
gtsam::Pose3 between(const gtsam::Pose3& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Pose3 retract(Vector v) const;
gtsam::Pose3 retractFirstOrder(Vector v) const;
Vector localCoordinates(const gtsam::Pose3& T2) const;
// Lie Group
@ -758,10 +739,6 @@ class CalibratedCamera {
gtsam::CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
// Group
gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
gtsam::CalibratedCamera inverse() const;
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
@ -1234,6 +1211,7 @@ class VectorValues {
#include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor {
gtsam::KeyVector keys() const;
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
@ -1657,6 +1635,7 @@ class NonlinearFactorGraph {
void push_back(gtsam::NonlinearFactor* factor);
void add(gtsam::NonlinearFactor* factor);
bool exists(size_t idx) const;
gtsam::KeySet keys() const;
// NonlinearFactorGraph
double error(const gtsam::Values& values) const;
@ -1727,6 +1706,7 @@ class Values {
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
void insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
@ -1737,10 +1717,14 @@ class Values {
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
void insert(size_t j, const gtsam::SimpleCamera& t);
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
// Fixed size version
void insertFixed(size_t j, Vector t, size_t n);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
@ -1757,6 +1741,16 @@ class Values {
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
T at(size_t j);
/// Fixed size versions, for n in 1..9
void insertFixed(size_t j, Vector t, size_t n);
/// Fixed size versions, for n in 1..9
Vector atFixed(size_t j, size_t n);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
};
// Actually a FastList<Key>
@ -2153,7 +2147,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = { gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2198,10 +2192,14 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
// Commented out by Frank Dec 2014: not poses!
// If needed, we need a RangeFactor that takes a camera, extracts the pose
// Should be easy with Expressions
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/BearingFactor.h>
@ -2373,8 +2371,6 @@ class ConstantBias {
gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::imuBias::ConstantBias retract(Vector v) const;
Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
@ -2393,25 +2389,24 @@ class ConstantBias {
}///\namespace imuBias
#include <gtsam/navigation/ImuFactor.h>
class PoseVelocity{
PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity);
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Matrix deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHat() const;
Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
@ -2419,10 +2414,11 @@ class ImuFactorPreintegratedMeasurements {
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
};
virtual class ImuFactor : gtsam::NonlinearFactor {
@ -2433,11 +2429,60 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
};
#include <gtsam/navigation/CombinedImuFactor.h>
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
// CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
double deltaTij() const;
Matrix deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
};
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
};
#include <gtsam/navigation/AHRSFactor.h>
class AHRSFactorPreintegratedMeasurements {
// Standard Constructor
@ -2450,7 +2495,6 @@ class AHRSFactorPreintegratedMeasurements {
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
// get Data
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
@ -2477,64 +2521,6 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
Vector omegaCoriolis) const;
};
#include <gtsam/navigation/CombinedImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
};
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
Matrix measurementCovariance() const;
Matrix deltaRij() const;
double deltaTij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHat() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix PreintMeasCov() const;
};
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis);
};
#include <gtsam/navigation/AttitudeFactor.h>
//virtual class AttitudeFactor : gtsam::NonlinearFactor {
// AttitudeFactor(const Unit3& nZ, const Unit3& bRef);

4
gtsam/3rdparty/metis/CMakeLists.txt vendored
View File

@ -2,14 +2,14 @@ cmake_minimum_required(VERSION 2.8)
project(METIS)
# Add flags for currect directory and below
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
add_definitions(-Wno-unused-variable)
if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0)
add_definitions(-Wno-sometimes-uninitialized)
endif()
endif()
if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC"))
if(NOT (${CMAKE_C_COMPILER_ID} MATCHES "MSVC" OR ${CMAKE_CXX_COMPILER_ID} MATCHES "MSVC"))
#add_definitions(-Wno-unknown-pragmas)
endif()

221
gtsam/base/ChartValue.h
View File

@ -1,221 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* @file ChartValue.h
* @brief
* @date October, 2014
* @author Michael Bosse, Abel Gawel, Renaud Dube
* based on DerivedValue.h by Duy Nguyen Ta
*/
#pragma once
#include <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 */

191
gtsam/base/GenericValue.h
View File

@ -19,78 +19,18 @@
#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>
#include <typeinfo> // operator typeid
namespace gtsam {
// To play as a GenericValue, we need the following traits
namespace traits {
// trait to wrap the default equals of types
template<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 +46,8 @@ protected:
T value_; ///< The wrapped value
public:
// Only needed for serialization.
GenericValue(){}
/// Construct from value
GenericValue(const T& value) :
@ -131,31 +73,124 @@ 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);
std::cout << "(" << typeid(T).name() << ") ";
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;

287
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,235 @@
namespace gtsam {
/// A CRTP helper class that implements Lie group methods
/// Prerequisites: methods operator*, inverse, and AdjointMap, as well as a
/// ChartAtOrigin struct that will be used to define the manifold Chart
/// To use, simply derive, but also say "using LieGroup<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,H2 ? &D_g_v : 0);
Class h = compose(g,H1,H2);
if (H2) *H2 = (*H2) * D_g_v;
return h;
}
TangentVector localCoordinates(const Class& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Class h = between(g,H1,H2);
Jacobian D_v_h;
TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = D_v_h * (*H1);
if (H2) *H2 = D_v_h * (*H2);
return v;
}
};
/// tag to assert a type is a Lie group
struct lie_group_tag: public manifold_tag, public group_tag {};
namespace internal {
/// A helper class that implements the traits interface for GTSAM lie groups.
/// To use this for your gtsam type, define:
/// template<> struct traits<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();}
/// @}
/// @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,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
return origin.localCoordinates(other, Horigin, Hother);
}
static Class Retract(const Class& origin, const TangentVector& v,
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
return origin.retract(v, Horigin, Hv);
}
/// @}
/// @name Lie Group
/// @{
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
return Class::Logmap(m, Hm);
}
static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
return Class::Expmap(v, Hv);
}
static Class Compose(const Class& m1, const Class& m2, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
return m1.compose(m2, H1, H2);
}
static Class Between(const Class& m1, const Class& m2, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
return m1.between(m2, H1, H2);
}
static Class Inverse(const Class& m, //
ChartJacobian H = boost::none) {
return m.inverse(H);
}
/// @}
};
} // \ namepsace internal
/**
* These core global functions can be specialized by new Lie types
* for better performance.
*/
/** Compute l0 s.t. l2=l1*l0 */
template<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 +302,5 @@ T expm(const Vector& x, int K=7) {
* NOTE: intentionally not in the gtsam namespace to allow for classes not in
* the gtsam namespace to be more easily enforced as testable
*/
#define GTSAM_CONCEPT_LIE_INST(T) \
template class gtsam::ManifoldConcept<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;

143
gtsam/base/LieMatrix.h
View File

@ -19,16 +19,19 @@
#include <cstdarg>
#ifdef _MSC_VER
#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.")
#else
#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#endif
#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.
*/
@ -36,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 */
@ -65,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:
@ -176,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

112
gtsam/base/LieScalar.h
View File

@ -17,10 +17,14 @@
#pragma once
#warning "LieScalar.h is deprecated. Please use double/float instead."
#ifdef _MSC_VER
#pragma message("LieScalar.h is deprecated. Please use double/float instead.")
#else
#warning "LieScalar.h is deprecated. Please use double/float instead."
#endif
#include <gtsam/dllexport.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/VectorSpace.h>
namespace gtsam {
@ -31,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_; }
@ -43,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 dexpL(const Vector& v) {
return eye(1);
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix dexpInvL(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

113
gtsam/base/LieVector.h
View File

@ -17,26 +17,34 @@
#pragma once
#ifdef _MSC_VER
#pragma message("LieVector.h is deprecated. Please use Eigen::Vector instead.")
#else
#warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
#include <gtsam/base/Lie.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/DerivedValue.h>
#endif
#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 */
@ -50,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:
@ -131,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, int 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)
{

67
gtsam/base/Matrix.h
View File

@ -37,36 +37,36 @@ namespace gtsam {
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
typedef Eigen::Matrix2d Matrix2;
typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix<double,5,5> Matrix5;
typedef Eigen::Matrix<double,6,6> Matrix6;
// Create handy typedefs and constants for square-size matrices
// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \
typedef Eigen::Matrix<double, SIZE, SIZE> Matrix##SUFFIX; \
typedef Eigen::Matrix<double, 1, SIZE> Matrix1##SUFFIX; \
typedef Eigen::Matrix<double, 2, SIZE> Matrix2##SUFFIX; \
typedef Eigen::Matrix<double, 3, SIZE> Matrix3##SUFFIX; \
typedef Eigen::Matrix<double, 4, SIZE> Matrix4##SUFFIX; \
typedef Eigen::Matrix<double, 5, SIZE> Matrix5##SUFFIX; \
typedef Eigen::Matrix<double, 6, SIZE> Matrix6##SUFFIX; \
typedef Eigen::Matrix<double, 7, SIZE> Matrix7##SUFFIX; \
typedef Eigen::Matrix<double, 8, SIZE> Matrix8##SUFFIX; \
typedef Eigen::Matrix<double, 9, SIZE> Matrix9##SUFFIX; \
static const Eigen::MatrixBase<Matrix##SUFFIX>::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \
static const Eigen::MatrixBase<Matrix##SUFFIX>::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero();
typedef Eigen::Matrix<double,2,3> Matrix23;
typedef Eigen::Matrix<double,2,4> Matrix24;
typedef Eigen::Matrix<double,2,5> Matrix25;
typedef Eigen::Matrix<double,2,6> Matrix26;
typedef Eigen::Matrix<double,2,7> Matrix27;
typedef Eigen::Matrix<double,2,8> Matrix28;
typedef Eigen::Matrix<double,2,9> Matrix29;
typedef Eigen::Matrix<double,3,2> Matrix32;
typedef Eigen::Matrix<double,3,4> Matrix34;
typedef Eigen::Matrix<double,3,5> Matrix35;
typedef Eigen::Matrix<double,3,6> Matrix36;
typedef Eigen::Matrix<double,3,7> Matrix37;
typedef Eigen::Matrix<double,3,8> Matrix38;
typedef Eigen::Matrix<double,3,9> Matrix39;
GTSAM_MAKE_TYPEDEFS(1,1);
GTSAM_MAKE_TYPEDEFS(2,2);
GTSAM_MAKE_TYPEDEFS(3,3);
GTSAM_MAKE_TYPEDEFS(4,4);
GTSAM_MAKE_TYPEDEFS(5,5);
GTSAM_MAKE_TYPEDEFS(6,6);
GTSAM_MAKE_TYPEDEFS(7,7);
GTSAM_MAKE_TYPEDEFS(8,8);
GTSAM_MAKE_TYPEDEFS(9,9);
// Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix;
// Two very commonly used matrices:
const Matrix3 Z_3x3 = Matrix3::Zero();
const Matrix3 I_3x3 = Matrix3::Identity();
// Matlab-like syntax
/**
@ -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>
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);

172
gtsam/base/OptionalJacobian.h Normal file
View File

@ -0,0 +1,172 @@
/* ----------------------------------------------------------------------------
* 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 OptionalJacobian.h
* @brief Special class for optional Jacobian arguments
* @author Frank Dellaert
* @author Natesh Srinivasan
* @date Nov 28, 2014
*/
#pragma once
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
#ifndef OPTIONALJACOBIAN_NOBOOST
#include <boost/optional.hpp>
#endif
namespace gtsam {
/**
* OptionalJacobian is an Eigen::Ref like class that can take be constructed using
* either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic
* 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:
/// ::Jacobian size type
typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
private:
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<Jacobian>(data);
}
public:
/// Default constructor acts like boost::none
OptionalJacobian() :
map_(NULL) {
}
/// Constructor that will usurp data of a fixed-size matrix
OptionalJacobian(Jacobian& fixed) :
map_(NULL) {
usurp(fixed.data());
}
/// Constructor that will usurp data of a fixed-size matrix, pointer version
OptionalJacobian(Jacobian* fixedPtr) :
map_(NULL) {
if (fixedPtr)
usurp(fixedPtr->data());
}
/// Constructor that will resize a dynamic matrix (unless already correct)
OptionalJacobian(Eigen::MatrixXd& dynamic) :
map_(NULL) {
dynamic.resize(Rows, Cols); // no malloc if correct size
usurp(dynamic.data());
}
#ifndef OPTIONALJACOBIAN_NOBOOST
/// Constructor with boost::none just makes empty
OptionalJacobian(boost::none_t none) :
map_(NULL) {
}
/// Constructor compatible with old-style derivatives
OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
map_(NULL) {
if (optional) {
optional->resize(Rows, Cols);
usurp(optional->data());
}
}
#endif
/// Return true is allocated, false if default constructor was used
operator bool() const {
return map_.data() != NULL;
}
/// De-reference, like boost optional
Eigen::Map<Jacobian>& operator*() {
return map_;
}
/// TODO: operator->()
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_!=NULL;
}
/// 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;

6
gtsam/base/Value.h
View File

@ -129,9 +129,6 @@ namespace gtsam {
template<typename ValueType>
const ValueType& cast() const;
template<typename Chart>
const Chart& getChart() const;
/** Virutal destructor */
virtual ~Value() {}
@ -168,7 +165,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) {
}
};

6
gtsam/base/Vector.cpp
View File

@ -286,11 +286,15 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights,
for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9);
// If there is a valid (a!=0) constraint (sigma==0) return the first one
for (size_t i = 0; i < m; ++i)
for (size_t i = 0; i < m; ++i) {
if (weights[i] == inf && !isZero[i]) {
// Basically, instead of doing a normal QR step with the weighted
// pseudoinverse, we enforce the constraint by turning
// ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0
pseudo = delta(m, i, 1 / a[i]);
return inf;
}
}
// Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma)
// For diagonal Sigma, inv(Sigma) = diag(precisions)

2
gtsam/base/Vector.h
View File

@ -34,6 +34,7 @@ namespace gtsam {
typedef Eigen::VectorXd Vector;
// Commonly used fixed size vectors
typedef Eigen::Matrix<double, 1, 1> Vector1;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 4, 1> Vector4;
@ -42,6 +43,7 @@ typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::Matrix<double, 7, 1> Vector7;
typedef Eigen::Matrix<double, 8, 1> Vector8;
typedef Eigen::Matrix<double, 9, 1> Vector9;
typedef Eigen::Matrix<double, 10, 1> Vector10;
typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector;

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

9
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;
@ -39,12 +42,6 @@ void testDefaultChart(TestResult& result_,
BOOST_CONCEPT_ASSERT((ChartConcept<Chart>));
T other = value;
// Check for the existence of a print function.
gtsam::traits::print<T>()(value, "value");
gtsam::traits::print<T>()(other, "other");
// Check for the existence of "equals"
EXPECT(gtsam::traits::equals<T>()(value, other, 1e-12));
// Check that the dimension of the local value matches the chart dimension.
Vector dx = Chart::local(value, other);

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

25
gtsam/base/debug.cpp
View File

@ -17,9 +17,32 @@
*/
#include <gtsam/base/debug.h>
#ifdef GTSAM_USE_TBB
#include <tbb/mutex.h>
#endif
namespace gtsam {
GTSAM_EXPORT FastMap<std::string, ValueWithDefault<bool,false> > debugFlags;
GTSAM_EXPORT FastMap<std::string, ValueWithDefault<bool, false> > debugFlags;
#ifdef GTSAM_USE_TBB
tbb::mutex debugFlagsMutex;
#endif
/* ************************************************************************* */
bool guardedIsDebug(const std::string& s) {
#ifdef GTSAM_USE_TBB
tbb::mutex::scoped_lock lock(debugFlagsMutex);
#endif
return gtsam::debugFlags[s];
}
/* ************************************************************************* */
void guardedSetDebug(const std::string& s, const bool v) {
#ifdef GTSAM_USE_TBB
tbb::mutex::scoped_lock lock(debugFlagsMutex);
#endif
gtsam::debugFlags[s] = v;
}
}

8
gtsam/base/debug.h
View File

@ -43,6 +43,10 @@
namespace gtsam {
GTSAM_EXTERN_EXPORT FastMap<std::string, ValueWithDefault<bool,false> > debugFlags;
// Non-guarded use led to crashes, and solved in commit cd35db2
bool GTSAM_EXPORT guardedIsDebug(const std::string& s);
void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v);
}
#undef ISDEBUG
@ -50,8 +54,8 @@ namespace gtsam {
#ifdef GTSAM_ENABLE_DEBUG
#define ISDEBUG(S) (gtsam::debugFlags[S])
#define SETDEBUG(S,V) ((void)(gtsam::debugFlags[S] = (V)))
#define ISDEBUG(S) (gtsam::guardedIsDebug(S))
#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V)))
#else

206
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,23 +279,65 @@ 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);
}
/**
* Compute numerical derivative in argument 1 of 4-argument function
* @param h ternary function yielding m-vector
* @param x1 n-dimensional first argument value
* @param x2 second argument value
* @param x3 third argument value
* @param x4 fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2, class X3, class X4>
typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative41(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
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( (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, x4), x1, delta);
}
/**
* Compute numerical derivative in argument 2 of 4-argument function
* @param h ternary function yielding m-vector
* @param x1 n-dimensional first argument value
* @param x2 second argument value
* @param x3 third argument value
* @param x4 fourth argument value
* @param delta increment for numerical derivative
* @return m*n Jacobian computed via central differencing
*/
template<class Y, class X1, class X2, class X3, class X4>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative42(
boost::function<Y(const X1&, const X2&, const X3&, const X4&)> h, const X1& x1,
const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) {
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( (boost::is_base_of<gtsam::manifold_tag, typename traits<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1, x3, x4), x2, delta);
}
/**
* Compute numerical Hessian matrix. Requires a single-argument Lie->scalar
* function. This is implemented simply as the derivative of the gradient.
@ -301,19 +347,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 +374,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 +386,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 +397,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 +420,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 +441,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 +452,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 +466,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 +475,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 +489,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 +498,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 +512,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 +521,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 +530,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 +539,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 +549,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 +557,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 +565,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,

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

@ -0,0 +1,85 @@
/* ----------------------------------------------------------------------------
* 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;
typedef OptionalJacobian<T::dimension,T::dimension> OJ;
// Inverse
OJ none;
EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1)));
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1)));
EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
// Compose
EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
// Between
EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), 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;
typedef typename G::TangentVector V;
typedef OptionalJacobian<T::dimension,T::dimension> OJ;
// Retract
OJ none;
V w12 = T::Local(t1, t2);
EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
// Local
EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2)));
EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), 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;

147
gtsam/base/tests/testOptionalJacobian.cpp Normal file
View File

@ -0,0 +1,147 @@
/* ----------------------------------------------------------------------------
* 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 testOptionalJacobian.cpp
* @brief Unit test for OptionalJacobian
* @author Frank Dellaert
* @date Nov 28, 2014
**/
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
TEST( OptionalJacobian, Constructors ) {
Matrix23 fixed;
OptionalJacobian<2, 3> H1;
EXPECT(!H1);
OptionalJacobian<2, 3> H2(fixed);
EXPECT(H2);
OptionalJacobian<2, 3> H3(&fixed);
EXPECT(H3);
Matrix dynamic;
OptionalJacobian<2, 3> H4(dynamic);
EXPECT(H4);
OptionalJacobian<2, 3> H5(boost::none);
EXPECT(!H5);
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);
}
//******************************************************************************
void test(OptionalJacobian<2, 3> H = boost::none) {
if (H)
*H = Matrix23::Zero();
}
void testPtr(Matrix23* H = NULL) {
if (H)
*H = Matrix23::Zero();
}
TEST( OptionalJacobian, Fixed) {
Matrix expected = Matrix23::Zero();
// Default argument does nothing
test();
// Fixed size, no copy
Matrix23 fixed1;
fixed1.setOnes();
test(fixed1);
EXPECT(assert_equal(expected,fixed1));
// Fixed size, no copy, pointer style
Matrix23 fixed2;
fixed2.setOnes();
test(&fixed2);
EXPECT(assert_equal(expected,fixed2));
// Empty is no longer a sign we don't want a matrix, we want it resized
Matrix dynamic0;
test(dynamic0);
EXPECT(assert_equal(expected,dynamic0));
// Dynamic wrong size
Matrix dynamic1(3, 5);
dynamic1.setOnes();
test(dynamic1);
EXPECT(assert_equal(expected,dynamic1));
// Dynamic right size
Matrix dynamic2(2, 5);
dynamic2.setOnes();
test(dynamic2);
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));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

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;

68
gtsam/geometry/Cal3Bundler.cpp
View File

@ -34,20 +34,22 @@ Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
}
/* ************************************************************************* */
Matrix Cal3Bundler::K() const {
Matrix3 Cal3Bundler::K() const {
Matrix3 K;
K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
return K;
}
/* ************************************************************************* */
Vector Cal3Bundler::k() const {
return (Vector(4) << k1_, k2_, 0, 0).finished();
Vector4 Cal3Bundler::k() const {
Vector4 rvalue_;
rvalue_ << k1_, k2_, 0, 0;
return rvalue_;
}
/* ************************************************************************* */
Vector3 Cal3Bundler::vector() const {
return (Vector(3) << f_, k1_, k2_).finished();
return Vector3(f_, k1_, k2_);
}
/* ************************************************************************* */
@ -64,21 +66,9 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
return true;
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
boost::optional<Matrix23&> Dcal, boost::optional<Matrix2&> Dp) const {
OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
@ -103,35 +93,6 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
boost::optional<Matrix&> Dcal, boost::optional<Matrix&> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
// Derivatives make use of intermediate variables above
if (Dcal) {
double rx = r * x, ry = r * y;
Dcal->resize(2, 3);
*Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
}
if (Dp) {
const double a = 2. * (k1_ + 2. * k2_ * r);
const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
Dp->resize(2,2);
*Dp << g + axx, axy, axy, g + ayy;
*Dp *= f_;
}
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
/* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
// Copied from Cal3DS2 :-(
@ -161,24 +122,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
Matrix Dp;
Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const {
Matrix2 Dp;
uncalibrate(p, boost::none, Dp);
return Dp;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
Matrix Dcal;
Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const {
Matrix23 Dcal;
uncalibrate(p, Dcal, boost::none);
return Dcal;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
Matrix Dcal, Dp;
Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
Matrix23 Dcal;
Matrix2 Dp;
uncalibrate(p, Dcal, Dp);
Matrix H(2, 5);
Matrix25 H;
H << Dp, Dcal;
return H;
}

55
gtsam/geometry/Cal3Bundler.h
View File

@ -37,6 +37,8 @@ private:
public:
enum { dimension = 3 };
/// @name Standard Constructors
/// @{
@ -69,8 +71,8 @@ public:
/// @name Standard Interface
/// @{
Matrix K() const; ///< Standard 3*3 calibration matrix
Vector k() const; ///< Radial distortion parameters (4 of them, 2 0)
Matrix3 K() const; ///< Standard 3*3 calibration matrix
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
Vector3 vector() const;
@ -106,43 +108,27 @@ public:
/**
* convert intrinsic coordinates xy to image coordinates uv
* @param p point in intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p) const;
/**
* Version of uncalibrate with fixed derivatives
* @brief: convert intrinsic coordinates xy to image coordinates uv
* Version of uncalibrate with derivatives
* @param p point in intrinsic coordinates
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix23&> Dcal,
boost::optional<Matrix2&> Dp) const;
/**
* Version of uncalibrate with dynamic derivatives
* @param p point in intrinsic coordinates
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
/// @deprecated might be removed in next release, use uncalibrate
Matrix D2d_intrinsic(const Point2& p) const;
Matrix2 D2d_intrinsic(const Point2& p) const;
/// @deprecated might be removed in next release, use uncalibrate
Matrix D2d_calibration(const Point2& p) const;
Matrix23 D2d_calibration(const Point2& p) const;
/// @deprecated might be removed in next release, use uncalibrate
Matrix D2d_intrinsic_calibration(const Point2& p) const;
Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
/// @}
/// @name Manifold
@ -185,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> {};
}

41
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -28,18 +28,22 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2_Base::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
Matrix3 Cal3DS2_Base::K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
/* ************************************************************************* */
Vector Cal3DS2_Base::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished();
Vector9 Cal3DS2_Base::vector() const {
Vector9 v;
v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_;
return v;
}
/* ************************************************************************* */
void Cal3DS2_Base::print(const std::string& s_) const {
gtsam::print(K(), s_ + ".K");
gtsam::print((Matrix)K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
}
@ -91,8 +95,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Matrix29&> H1,
boost::optional<Matrix2&> H2) const {
OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
@ -126,26 +129,6 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1 || H2) {
Matrix29 D1;
Matrix2 D2;
Point2 pu = uncalibrate(p, D1, D2);
if (H1)
*H1 = D1;
if (H2)
*H2 = D2;
return pu;
} else {
return uncalibrate(p);
}
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
@ -177,7 +160,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
@ -188,7 +171,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr * rr;

19
gtsam/geometry/Cal3DS2_Base.h
View File

@ -45,9 +45,9 @@ protected:
double p1_, p2_ ; // tangential distortion
public:
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
Matrix3 K() const ;
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
Vector9 vector() const ;
/// @name Standard Constructors
/// @{
@ -113,23 +113,18 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix29&> Dcal = boost::none,
boost::optional<Matrix2&> Dp = boost::none) const ;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const ;
OptionalJacobian<2,9> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix D2d_intrinsic(const Point2& p) const ;
Matrix2 D2d_intrinsic(const Point2& p) const ;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
Matrix29 D2d_calibration(const Point2& p) const ;
private:

19
gtsam/geometry/Cal3Unified.cpp
View File

@ -29,8 +29,10 @@ Cal3Unified::Cal3Unified(const Vector &v):
Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
/* ************************************************************************* */
Vector Cal3Unified::vector() const {
return (Vector(10) << Base::vector(), xi_).finished();
Vector10 Cal3Unified::vector() const {
Vector10 v;
v << Base::vector(), xi_;
return v;
}
/* ************************************************************************* */
@ -52,8 +54,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
/* ************************************************************************* */
// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
OptionalJacobian<2,10> H1,
OptionalJacobian<2,2> H2) const {
// this part of code is modified from Cal3DS2,
// since the second part of this model (after project to normalized plane)
@ -81,10 +83,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
Vector2 DU;
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2;
H1->resize(2,10);
H1->block<2,9>(0,0) = H1base;
H1->block<2,1>(0,9) = H2base * DU;
*H1 << H1base, H2base * DU;
}
// Inlined derivative for points
@ -96,7 +95,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
*H2 = H2base * DU;
*H2 << H2base * DU;
}
return puncalib;
@ -136,7 +135,7 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
}
/* ************************************************************************* */
Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
return T2.vector() - vector();
}

26
gtsam/geometry/Cal3Unified.h
View File

@ -50,8 +50,9 @@ private:
double xi_; // mirror parameter
public:
enum { dimension = 10 };
Vector vector() const ;
Vector10 vector() const ;
/// @name Standard Constructors
/// @{
@ -96,8 +97,8 @@ public:
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
OptionalJacobian<2,10> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const ;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
@ -116,7 +117,7 @@ public:
Cal3Unified retract(const Vector& d) const ;
/// Given a different calibration, calculate update to obtain it
Vector localCoordinates(const Cal3Unified& T2) const ;
Vector10 localCoordinates(const Cal3Unified& T2) const ;
/// Return dimensions of calibration manifold object
virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
@ -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> {};
}

29
gtsam/geometry/Cal3_S2.cpp
View File

@ -53,7 +53,7 @@ Cal3_S2::Cal3_S2(const std::string &path) :
/* ************************************************************************* */
void Cal3_S2::print(const std::string& s) const {
gtsam::print(matrix(), s);
gtsam::print((Matrix)matrix(), s);
}
/* ************************************************************************* */
@ -72,32 +72,13 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const {
Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
OptionalJacobian<2, 2> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal) {
Dcal->resize(2,5);
if (Dcal)
*Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
}
if (Dp) {
Dp->resize(2,2);
if (Dp)
*Dp << fx_, s_, 0.0, fy_;
}
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
boost::optional<Matrix2&> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
if (Dp) *Dp << fx_, s_, 0.0, fy_;
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p) const {
const double x = p.x(), y = p.y();
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}

73
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
@ -117,37 +117,33 @@ public:
}
/// vectorized form (column-wise)
Vector vector() const {
double r[] = { fx_, fy_, s_, u0_, v0_ };
Vector v(5);
std::copy(r, r + 5, v.data());
Vector5 vector() const {
Vector5 v;
v << fx_, fy_, s_, u0_, v0_;
return v;
}
/// return calibration matrix K
Matrix K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
Matrix3 K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
/** @deprecated The following function has been deprecated, use K above */
Matrix matrix() const {
Matrix3 matrix() const {
return K();
}
/// return inverted calibration matrix inv(K)
Matrix matrix_inverse() const {
Matrix3 matrix_inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished();
Matrix3 K_inverse;
K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
return K_inverse;
}
/**
* convert intrinsic coordinates xy to image coordinates uv
* @param p point in intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p) const;
/**
* convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
@ -155,18 +151,8 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
boost::optional<Matrix2&> Dp) const;
/**
* convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const;
/**
* convert image coordinates uv to intrinsic coordinates xy
@ -184,10 +170,10 @@ public:
/// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
inline Cal3_S2 between(const Cal3_S2& q,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(5);
if(H2) *H2 = eye(5);
OptionalJacobian<5,5> H1=boost::none,
OptionalJacobian<5,5> H2=boost::none) const {
if(H1) *H1 = -I_5x5;
if(H2) *H2 = I_5x5;
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
}
@ -212,7 +198,7 @@ public:
}
/// Unretraction for the calibration
Vector localCoordinates(const Cal3_S2& T2) const {
Vector5 localCoordinates(const Cal3_S2& T2) const {
return T2.vector() - vector();
}
@ -237,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

39
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
@ -103,6 +104,38 @@ namespace gtsam {
/// return baseline
inline double baseline() const { return b_; }
/// vectorized form (column-wise)
Vector6 vector() const {
Vector6 v;
v << K_.vector(), b_;
return v;
}
/// @}
/// @name Manifold
/// @{
/// return DOF, dimensionality of tangent space
inline size_t dim() const {
return 6;
}
/// return DOF, dimensionality of tangent space
static size_t Dim() {
return 6;
}
/// Given 6-dim tangent vector, create new calibration
inline Cal3_S2Stereo retract(const Vector& d) const {
return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
}
/// Unretraction for the calibration
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
return T2.vector() - vector();
}
/// @}
/// @name Advanced Interface
/// @{
@ -119,4 +152,10 @@ namespace gtsam {
/// @}
};
// Define GTSAM traits
template<>
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
} // \ namespace gtsam

33
gtsam/geometry/CalibratedCamera.cpp
View File

@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) :
/* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P,
boost::optional<Matrix&> H1) {
OptionalJacobian<2,3> H1) {
if (H1) {
double d = 1.0 / P.z(), d2 = d * d;
*H1 = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2).finished();
*H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
@ -59,10 +59,12 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const {
OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
Point3 q = pose_.transform_to(point, Dpose, Dpoint);
Matrix36 Dpose_;
Matrix3 Dpoint_;
Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
#else
Point3 q = pose_.transform_to(point);
#endif
@ -75,23 +77,26 @@ Point2 CalibratedCamera::project(const Point3& point,
if (Dpose || Dpoint) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
// just implement chain rule
Matrix H;
project_to_camera(q,H);
if (Dpose) *Dpose = H * (*Dpose);
if (Dpoint) *Dpoint = H * (*Dpoint);
if(Dpose && Dpoint) {
Matrix23 H;
project_to_camera(q,H);
if (Dpose) *Dpose = H * (*Dpose_);
if (Dpoint) *Dpoint = H * (*Dpoint_);
}
#else
// optimized version, see CalibratedCamera.nb
const double z = q.z(), d = 1.0 / z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
if (Dpose)
*Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-uv, -u, 0., -d, d * v).finished();
*Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-uv, -u, 0., -d, d * v;
if (Dpoint) {
const Matrix R(pose_.rotation().matrix());
*Dpoint = d
* (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
const Matrix3 R(pose_.rotation().matrix());
Matrix23 Dpoint_;
Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished();
R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
*Dpoint = d * Dpoint_;
}
#endif
}

59
gtsam/geometry/CalibratedCamera.h
View File

@ -18,9 +18,8 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -45,6 +44,8 @@ private:
public:
enum { dimension = 6 };
/// @name Standard Constructors
/// @{
@ -88,26 +89,6 @@ public:
return pose_;
}
/// compose the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera compose(const CalibratedCamera &c,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
}
/// between the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera between(const CalibratedCamera& c,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
return CalibratedCamera(pose_.between(c.pose(), H1, H2));
}
/// invert the camera pose: TODO Frank says this might not make sense
inline const CalibratedCamera inverse(boost::optional<Matrix&> H1 =
boost::none) const {
return CalibratedCamera(pose_.inverse(H1));
}
/**
* Create a level camera at the given 2D pose and height
* @param pose2 specifies the location and viewing direction
@ -152,8 +133,8 @@ public:
* @return the intrinsic coordinates of the projected point
*/
Point2 project(const Point3& point,
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const;
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
/**
* projects a 3-dimensional point in camera coordinates into the
@ -161,7 +142,7 @@ public:
* With optional 2by3 derivative
*/
static Point2 project_to_camera(const Point3& cameraPoint,
boost::optional<Matrix&> H1 = boost::none);
OptionalJacobian<2, 3> H1 = boost::none);
/**
* backproject a 2-dimensional point to a 3-dimension point
@ -175,8 +156,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Point3& point, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) const {
return pose_.range(point, H1, H2);
}
@ -187,8 +168,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Pose3& pose, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(pose, H1, H2);
}
@ -199,8 +180,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const CalibratedCamera& camera, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
return pose_.range(camera.pose_, H1, H2);
}
@ -220,22 +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

53
gtsam/geometry/EssentialMatrix.cpp
View File

@ -14,7 +14,7 @@ namespace gtsam {
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
boost::optional<Matrix&> H) {
OptionalJacobian<5, 6> H) {
const Rot3& _1R2_ = _1P2_.rotation();
const Point3& _1T2_ = _1P2_.translation();
if (!H) {
@ -25,13 +25,10 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
// Calculate the 5*6 Jacobian H = D_E_1P2
// D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
// First get 2*3 derivative from Unit3::FromPoint3
Matrix D_direction_1T2;
Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
H->resize(5, 6);
H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right
H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
*H << I_3x3, Z_3x3, //
Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
return EssentialMatrix(_1R2_, direction);
}
}
@ -54,23 +51,26 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
}
/* ************************************************************************* */
Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
return (Vector(5) <<
aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished();
Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
Vector5 v;
v << aRb_.localCoordinates(other.aRb_),
aTb_.localCoordinates(other.aTb_);
return v;
}
/* ************************************************************************* */
Point3 EssentialMatrix::transform_to(const Point3& p,
boost::optional<Matrix&> DE, boost::optional<Matrix&> Dpoint) const {
Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
OptionalJacobian<3, 3> Dpoint) const {
Pose3 pose(aRb_, aTb_.point3());
Point3 q = pose.transform_to(p, DE, Dpoint);
Matrix36 DE_;
Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// Duy made an educated guess that this needs to be rotated to the local frame
Matrix H(3, 5);
H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
Matrix35 H;
H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
*DE = H;
}
return q;
@ -78,7 +78,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p,
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
boost::optional<Matrix&> HE, boost::optional<Matrix&> HR) const {
OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const {
// The rotation must be conjugated to act in the camera frame
Rot3 c1Rc2 = aRb_.conjugate(cRb);
@ -89,18 +89,16 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
return EssentialMatrix(c1Rc2, c1Tc2);
} else {
// Calculate derivatives
Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
Matrix23 D_c1Tc2_cRb; // 2*3
Matrix2 D_c1Tc2_aTb; // 2*2
Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE) {
*HE = zeros(5, 5);
HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
}
if (HE)
*HE << cRb.matrix(), Matrix32::Zero(), //
Matrix23::Zero(), D_c1Tc2_aTb;
if (HR) {
throw runtime_error(
"EssentialMatrix::rotate: derivative HR not implemented yet");
/*
HR->resize(5, 3);
HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
*/
@ -110,13 +108,12 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
}
/* ************************************************************************* */
double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
boost::optional<Matrix&> H) const {
double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
OptionalJacobian<1, 5> H) const {
if (H) {
H->resize(1, 5);
// See math.lyx
Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis();
*H << HR, HD;
}

43
gtsam/geometry/EssentialMatrix.h
View File

@ -30,9 +30,11 @@ private:
public:
enum { dimension = 5 };
/// Static function to convert Point2 to homogeneous coordinates
static Vector Homogeneous(const Point2& p) {
return (Vector(3) << p.x(), p.y(), 1).finished();
static Vector3 Homogeneous(const Point2& p) {
return Vector3(p.x(), p.y(), 1);
}
/// @name Constructors and named constructors
@ -50,7 +52,7 @@ public:
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
static EssentialMatrix FromPose3(const Pose3& _1P2_,
boost::optional<Matrix&> H = boost::none);
OptionalJacobian<5, 6> H = boost::none);
/// Random, using Rot3::Random and Unit3::Random
template<typename Engine>
@ -84,15 +86,15 @@ public:
}
/// Return the dimensionality of the tangent space
virtual size_t dim() const {
size_t dim() const {
return 5;
}
/// Retract delta to manifold
virtual EssentialMatrix retract(const Vector& xi) const;
EssentialMatrix retract(const Vector& xi) const;
/// Compute the coordinates in the tangent space
virtual Vector localCoordinates(const EssentialMatrix& other) const;
Vector5 localCoordinates(const EssentialMatrix& other) const;
/// @}
@ -132,16 +134,16 @@ public:
* @return point in pose coordinates
*/
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> DE = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const;
OptionalJacobian<3,5> DE = boost::none,
OptionalJacobian<3,3> Dpoint = boost::none) const;
/**
* Given essential matrix E in camera frame B, convert to body frame C
* @param cRb rotation from body frame to camera frame
* @param E essential matrix E in camera frame C
*/
EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
boost::none, boost::optional<Matrix&> HR = boost::none) const;
EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
/**
* Given essential matrix E in camera frame B, convert to body frame C
@ -153,8 +155,8 @@ public:
}
/// epipolar error, algebraic
double error(const Vector& vA, const Vector& vB, //
boost::optional<Matrix&> H = boost::none) const;
double error(const Vector3& vA, const Vector3& vB, //
OptionalJacobian<1,5> H = boost::none) const;
/// @}
@ -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

354
gtsam/geometry/PinholeCamera.h
View File

@ -18,16 +18,9 @@
#pragma once
#include <cmath>
#include <boost/optional.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <cmath>
namespace gtsam {
@ -38,14 +31,20 @@ namespace gtsam {
*/
template<typename Calibration>
class PinholeCamera {
private:
Pose3 pose_;
Calibration K_;
static const int DimK = traits::dimension<Calibration>::value;
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
/// @{
@ -114,9 +113,9 @@ public:
/// @{
explicit PinholeCamera(const Vector &v) {
pose_ = Pose3::Expmap(v.head(Pose3::Dim()));
if (v.size() > Pose3::Dim()) {
K_ = Calibration(v.tail(Calibration::Dim()));
pose_ = Pose3::Expmap(v.head(6));
if (v.size() > 6) {
K_ = Calibration(v.tail(DimK));
}
}
@ -167,99 +166,43 @@ public:
return K_;
}
/// @}
/// @name Group ?? Frank says this might not make sense
/// @{
/// compose two cameras: TODO Frank says this might not make sense
inline const PinholeCamera compose(const PinholeCamera &c,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_);
if (H1) {
H1->conservativeResize(Dim(), Dim());
H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
}
if (H2) {
H2->conservativeResize(Dim(), Dim());
H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
}
return result;
}
/// between two cameras: TODO Frank says this might not make sense
inline const PinholeCamera between(const PinholeCamera& c,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
PinholeCamera result(pose_.between(c.pose(), H1, H2), K_);
if (H1) {
H1->conservativeResize(Dim(), Dim());
H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
}
if (H2) {
H2->conservativeResize(Dim(), Dim());
H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
}
return result;
}
/// inverse camera: TODO Frank says this might not make sense
inline const PinholeCamera inverse(
boost::optional<Matrix&> H1 = boost::none) const {
PinholeCamera result(pose_.inverse(H1), K_);
if (H1) {
H1->conservativeResize(Dim(), Dim());
H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
}
return result;
}
/// compose two cameras: TODO Frank says this might not make sense
inline const PinholeCamera compose(const Pose3 &c) const {
return PinholeCamera(pose_.compose(c), K_);
}
/// @}
/// @name Manifold
/// @{
/// Manifold dimension
inline size_t dim() const {
return dimension;
}
/// Manifold dimension
inline static size_t Dim() {
return dimension;
}
typedef Eigen::Matrix<double, dimension, 1> VectorK6;
/// move a cameras according to d
PinholeCamera retract(const Vector& d) const {
if ((size_t) d.size() == pose_.dim())
if ((size_t) d.size() == 6)
return PinholeCamera(pose().retract(d), calibration());
else
return PinholeCamera(pose().retract(d.head(pose().dim())),
return PinholeCamera(pose().retract(d.head(6)),
calibration().retract(d.tail(calibration().dim())));
}
typedef Eigen::Matrix<double,6+DimK,1> VectorK6;
/// 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;
}
/// Manifold dimension
inline size_t dim() const {
return pose_.dim() + K_.dim();
}
/// Manifold dimension
inline static size_t Dim() {
return Pose3::Dim() + Calibration::Dim();
/// for Canonical
static PinholeCamera identity() {
return PinholeCamera(); // assumes that the default constructor is valid
}
/// @}
@ -272,8 +215,8 @@ public:
* @param P A point in camera coordinates
* @param Dpoint is the 2*3 Jacobian w.r.t. P
*/
inline static Point2 project_to_camera(const Point3& P,
boost::optional<Matrix23&> Dpoint = boost::none) {
static Point2 project_to_camera(const Point3& P, //
OptionalJacobian<2, 3> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0)
throw CheiralityException();
@ -292,21 +235,7 @@ public:
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
}
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
*/
inline Point2 project(const Point3& pw) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
// Project to normalized image coordinates
const Point2 pn = project_to_camera(pc);
return K_.uncalibrate(pn);
}
typedef Eigen::Matrix<double,2,DimK> Matrix2K;
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
@ -314,11 +243,9 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
inline Point2 project(
const Point3& pw, //
boost::optional<Matrix26&> Dpose,
boost::optional<Matrix23&> Dpoint,
boost::optional<Matrix2K&> Dcal) const {
inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
@ -340,46 +267,7 @@ public:
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
return pi;
} else
return K_.uncalibrate(pn, Dcal, boost::none);
}
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
inline Point2 project(
const Point3& pw, //
boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint,
boost::optional<Matrix&> Dcal) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
// Project to normalized image coordinates
const Point2 pn = project_to_camera(pc);
if (Dpose || Dpoint) {
const double z = pc.z(), d = 1.0 / z;
// uncalibration
Matrix Dpi_pn(2, 2);
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
if (Dpose) {
Dpose->resize(2, 6);
calculateDpose(pn, d, Dpi_pn, *Dpose);
}
if (Dpoint) {
Dpoint->resize(2, 3);
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
}
return pi;
} else
return K_.uncalibrate(pn, Dcal, boost::none);
return K_.uncalibrate(pn, Dcal);
}
/** project a point at infinity from world coordinate to the image
@ -388,11 +276,10 @@ public:
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
inline Point2 projectPointAtInfinity(
const Point3& pw, //
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpoint = boost::none,
boost::optional<Matrix&> Dcal = boost::none) const {
inline Point2 projectPointAtInfinity(const Point3& pw,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
@ -401,40 +288,30 @@ public:
}
// world to camera coordinate
Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */;
Matrix3 Dpc_rot, Dpc_point;
const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
Matrix Dpc_pose = Matrix::Zero(3, 6);
Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
Matrix36 Dpc_pose;
Dpc_pose.setZero();
Dpc_pose.leftCols<3>() = Dpc_rot;
// camera to normalized image coordinate
Matrix23 Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc);
// uncalibration
Matrix Dpi_pn; // 2*2
Matrix2 Dpi_pn; // 2*2
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
if (Dpose)
*Dpose = Dpi_pc * Dpc_pose;
if (Dpoint)
*Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only)
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
return pi;
}
typedef Eigen::Matrix<double,2,6+DimK> Matrix2K6;
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinate
*/
Point2 project2(const Point3& pw) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
return K_.uncalibrate(pn);
}
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
@ -442,8 +319,8 @@ public:
*/
Point2 project2(
const Point3& pw, //
boost::optional<Matrix2K6&> Dcamera,
boost::optional<Matrix23&> Dpoint) const {
OptionalJacobian<2, dimension> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
@ -459,8 +336,8 @@ public:
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) { // TODO why does leftCols<6>() not compile ??
calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6));
Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
(*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib
}
if (Dpoint) {
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
@ -469,40 +346,6 @@ public:
}
}
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param Dpoint is the Jacobian w.r.t. point3
*/
Point2 project2(const Point3& pw, //
boost::optional<Matrix&> Dcamera, boost::optional<Matrix&> Dpoint) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
if (!Dcamera && !Dpoint) {
return K_.uncalibrate(pn);
} else {
const double z = pc.z(), d = 1.0 / z;
// uncalibration
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) {
Dcamera->resize(2, this->dim());
calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>());
Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
}
if (Dpoint) {
Dpoint->resize(2, 3);
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
}
return pi;
}
}
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
inline Point3 backproject(const Point2& p, double depth) const {
const Point2 pn = K_.calibrate(p);
@ -520,71 +363,64 @@ public:
/**
* Calculate range to a landmark
* @param point 3D location of landmark
* @param Dpose the optionally computed Jacobian with respect to pose
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpoint the optionally computed Jacobian with respect to the landmark
* @return range (double)
*/
double range(
const Point3& point, //
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const {
double result = pose_.range(point, Dpose, Dpoint);
if (Dpose) {
// Add columns of zeros to Jacobian for calibration
Matrix& H1r(*Dpose);
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
}
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result;
}
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @param Dpose the optionally computed Jacobian with respect to pose
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dpose2 the optionally computed Jacobian with respect to the other pose
* @return range (double)
*/
double range(
const Pose3& pose, //
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpose2 = boost::none) const {
double result = pose_.range(pose, Dpose, Dpose2);
if (Dpose) {
// Add columns of zeros to Jacobian for calibration
Matrix& H1r(*Dpose);
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
}
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
Matrix16 Dpose_;
double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result;
}
/**
* Calculate range to another camera
* @param camera Other camera
* @param Dpose the optionally computed Jacobian with respect to pose
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double)
*/
template<class CalibrationB>
double range(
const PinholeCamera<CalibrationB>& camera, //
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dother = boost::none) const {
double result = pose_.range(camera.pose_, Dpose, Dother);
if (Dpose) {
// Add columns of zeros to Jacobian for calibration
Matrix& H1r(*Dpose);
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
OptionalJacobian<1, dimension> Dcamera = boost::none,
// OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
boost::optional<Matrix&> Dother =
boost::none) const {
Matrix16 Dcamera_, Dother_;
double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0);
if (Dcamera) {
Dcamera->resize(1, 6 + DimK);
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
}
if (Dother) {
// Add columns of zeros to Jacobian for calibration
Matrix& H2r(*Dother);
H2r.conservativeResize(Eigen::NoChange,
camera.pose().dim() + camera.calibration().dim());
H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) =
Matrix::Zero(1, camera.calibration().dim());
Dother->resize(1, 6+CalibrationB::dimension);
Dother->setZero();
Dother->block(0, 0, 1, 6) = Dother_;
}
return result;
}
@ -592,15 +428,15 @@ public:
/**
* Calculate range to another camera
* @param camera Other camera
* @param Dpose the optionally computed Jacobian with respect to pose
* @param Dcamera the optionally computed Jacobian with respect to pose
* @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double)
*/
double range(
const CalibratedCamera& camera, //
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dother = boost::none) const {
return pose_.range(camera.pose_, Dpose, Dother);
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
return range(camera.pose(), Dcamera, Dother);
}
private:
@ -659,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

21
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;
@ -38,15 +34,9 @@ bool Point2::equals(const Point2& q, double tol) const {
}
/* ************************************************************************* */
double Point2::norm() const {
return sqrt(x_ * x_ + y_ * y_);
}
/* ************************************************************************* */
double Point2::norm(boost::optional<Matrix&> H) const {
double r = norm();
double Point2::norm(OptionalJacobian<1,2> H) const {
double r = sqrt(x_ * x_ + y_ * y_);
if (H) {
H->resize(1,2);
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r;
else
@ -56,12 +46,11 @@ double Point2::norm(boost::optional<Matrix&> H) const {
}
/* ************************************************************************* */
static const Matrix I2 = eye(2);
double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const {
Point2 d = point - *this;
if (H1 || H2) {
Matrix H;
Matrix12 H;
double r = d.norm(H);
if (H1) *H1 = -H;
if (H2) *H2 = H;

106
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/Matrix.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,66 +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,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = eye(2);
if(H2) *H2 = eye(2);
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,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(2);
if(H2) *H2 = eye(2);
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 Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); }
/// @}
/// @name Vector Space
/// @{
@ -180,15 +125,12 @@ public:
/** creates a unit vector */
Point2 unit() const { return *this/norm(); }
/** norm of point */
double norm() const;
/** norm of point, with derivative */
double norm(boost::optional<Matrix&> H) const;
double norm(OptionalJacobian<1,2> H = boost::none) const;
/** distance between two points */
double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
OptionalJacobian<1,2> H2 = boost::none) const;
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point2& p2) const {
@ -218,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() const { return -(*this);}
Point2 compose(const Point2& q) const { return (*this)+q;}
Point2 between(const Point2& q) const { return q-(*this);}
Vector2 localCoordinates(const Point2& q) const { return between(q).vector();}
Point2 retract(const Vector2& v) const { return compose(Point2(v));}
static Vector2 Logmap(const Point2& p) { return p.vector();}
static Point2 Expmap(const Vector2& v) { return Point2(v);}
/// @}
/// Streaming
@ -248,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

49
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
@ -63,22 +59,18 @@ Point3 Point3::operator/(double s) const {
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = eye(3, 3);
if (H2)
*H2 = eye(3, 3);
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3;
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = eye(3, 3);
if (H2)
*H2 = -eye(3, 3);
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3;
return *this - q;
}
@ -94,26 +86,8 @@ double Point3::dot(const Point3 &q) const {
}
/* ************************************************************************* */
double Point3::norm() const {
return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
}
/* ************************************************************************* */
double Point3::norm(boost::optional<Matrix&> H) const {
double r = norm();
if (H) {
H->resize(1,3);
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r, z_ / r;
else
*H << 1, 1, 1; // really infinity, why 1 ?
}
return r;
}
/* ************************************************************************* */
double Point3::norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const {
double r = norm();
double Point3::norm(OptionalJacobian<1,3> H) const {
double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
if (H) {
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r, z_ / r;
@ -124,13 +98,12 @@ double Point3::norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const {
}
/* ************************************************************************* */
Point3 Point3::normalize(boost::optional<Matrix&> H) const {
Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
Point3 normalized = *this / norm();
if (H) {
// 3*3 Derivative
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
H->resize(3, 3);
*H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
*H /= pow(x2 + y2 + z2, 1.5);
}

127
gtsam/geometry/Point3.h
View File

@ -21,12 +21,8 @@
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.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,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if (H1) *H1 = eye(3);
if (H2) *H2 = eye(3);
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,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(3);
if(H2) *H2 = eye(3);
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 Matrix dexpL(const Vector& v) {
return eye(3);
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix dexpInvL(const Vector& v) {
return eye(3);
}
/// @}
/// @name Vector Space
/// @{
@ -163,14 +99,16 @@ namespace gtsam {
/** distance between two points */
inline double distance(const Point3& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const {
double d = (p2 - *this).norm();
if (H1) {
*H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z()).finished()*(1./d);
*H1 << x_-p2.x(), y_-p2.y(), z_-p2.z();
*H1 = *H1 *(1./d);
}
if (H2) {
*H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z()).finished()*(1./d);
*H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
*H2 << *H2 *(1./d);
}
return d;
}
@ -180,17 +118,11 @@ namespace gtsam {
return (p2 - *this).norm();
}
/** Distance of the point from the origin */
double norm() const;
/** Distance of the point from the origin, with Jacobian */
double norm(boost::optional<Matrix&> H) const;
/** Distance of the point from the origin, with Jacobian */
double norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const;
double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q) const;
@ -219,17 +151,28 @@ namespace gtsam {
/** add two points, add(this,q) is same as this + q */
Point3 add (const Point3 &q,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const;
/** subtract two points, sub(this,q) is same as this - q */
Point3 sub (const Point3 &q,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
/// @}
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p);
/// @name Deprecated
/// @{
Point3 inverse() const { return -(*this);}
Point3 compose(const Point3& q) const { return (*this)+q;}
Point3 between(const Point3& q) const { return q-(*this);}
Vector3 localCoordinates(const Point3& q) const { return between(q).vector();}
Point3 retract(const Vector3& v) const { return compose(Point3(v));}
static Vector3 Logmap(const Point3& p) { return p.vector();}
static Point3 Expmap(const Vector3& v) { return Point3(v);}
/// @}
private:
/// @name Advanced Interface
@ -252,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> {};
}

335
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,21 +29,23 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose2);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2);
static const Matrix I3 = eye(3), Z12 = zeros(1,2);
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
/* ************************************************************************* */
Matrix Pose2::matrix() const {
Matrix R = r_.matrix();
R = stack(2, &R, &Z12);
Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0).finished();
return collect(2, &R, &T);
Matrix3 Pose2::matrix() const {
Matrix2 R = r_.matrix();
Matrix32 R0;
R0.block<2,2>(0,0) = R;
R0.block<1,2>(2,0).setZero();
Matrix31 T;
T << t_.x(), t_.y(), 1.0;
Matrix3 RT_;
RT_.block<3,2>(0,0) = R0;
RT_.block<3,1>(0,2) = T;
return RT_;
}
/* ************************************************************************* */
@ -55,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);
@ -70,260 +75,222 @@ Pose2 Pose2::Expmap(const Vector& xi) {
}
/* ************************************************************************* */
Vector 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();
if (std::abs(w) < 1e-10)
return (Vector(3) << t.x(), t.y(), w).finished();
return Vector3(t.x(), t.y(), w);
else {
double c_1 = R.c()-1.0, s = R.s();
double det = c_1*c_1 + s*s;
Point2 p = R_PI_2 * (R.unrotate(t) - t);
Point2 v = (w/det) * p;
return (Vector(3) << v.x(), v.y(), w).finished();
return Vector3(v.x(), v.y(), w);
}
}
/* ************************************************************************* */
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
}
/* ************************************************************************* */
Vector 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);
return (Vector(3) << r.x(), r.y(), r.theta()).finished();
if (H) {
*H = I_3x3;
H->topLeftCorner<2,2>() = r.rotation().matrix();
}
return Vector3(r.x(), r.y(), r.theta());
#endif
}
/* ************************************************************************* */
// Calculate Adjoint map
// Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
Matrix Pose2::AdjointMap() const {
Matrix3 Pose2::AdjointMap() const {
double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
return (Matrix(3, 3) <<
Matrix3 rvalue;
rvalue <<
c, -s, y,
s, c, -x,
0.0, 0.0, 1.0
).finished();
0.0, 0.0, 1.0;
return rvalue;
}
/* ************************************************************************* */
Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -AdjointMap();
Matrix3 Pose2::adjointMap(const Vector& v) {
// See Chirikjian12book2, vol.2, pg. 36
Matrix3 ad = zeros(3,3);
ad(0,1) = -v[2];
ad(1,0) = v[2];
ad(0,2) = v[1];
ad(1,2) = -v[0];
return ad;
}
/* ************************************************************************* */
Matrix3 Pose2::ExpmapDerivative(const Vector3& v) {
double alpha = v[2];
Matrix3 J;
if (fabs(alpha) > 1e-5) {
// Chirikjian11book2, pg. 36
/* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26
* Iserles' right-trivialization dexpR is actually the left Jacobian J_l in Chirikjian's notation
* In fact, Iserles 2.42 can be written as:
* \dot{g} g^{-1} = dexpR_{q}\dot{q}
* where q = A, and g = exp(A)
* and the LHS is in the definition of J_l in Chirikjian11book2, pg. 26.
* Hence, to compute ExpmapDerivative, we have to use the formula of J_r Chirikjian11book2, pg.36
*/
double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha;
double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha;
J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha,
c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha,
0, 0, 1;
}
else {
// Thanks to Krunal: Apply L'Hospital rule to several times to
// compute the limits when alpha -> 0
J << 1,0,-0.5*v[1],
0,1, 0.5*v[0],
0,0, 1;
}
return J;
}
/* ************************************************************************* */
Matrix3 Pose2::LogmapDerivative(const Pose2& p) {
Vector3 v = Logmap(p);
double alpha = v[2];
Matrix3 J;
if (fabs(alpha) > 1e-5) {
double alphaInv = 1/alpha;
double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha));
double v1 = v[0], v2 = v[1];
J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2,
0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha,
0, 0, 1;
}
else {
J << 1,0, 0.5*v[1],
0,1, -0.5*v[0],
0,0, 1;
}
return J;
}
/* ************************************************************************* */
Pose2 Pose2::inverse() const {
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point) const {
Point2 d = point - t_;
return r_.unrotate(d);
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
boost::optional<Matrix23&> H1, boost::optional<Matrix2&> H2) const {
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x();
if (H2) *H2 = r_.transpose();
if (H2) *H2 << r_.transpose();
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 = (Matrix(2, 3) <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x()).finished();
if (H2) *H2 = r_.transpose();
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// TODO: inline and reuse?
if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I3;
return (*this)*p2;
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_from(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
const Point2 q = r_ * p;
if (H1 || H2) {
const Matrix R = r_.matrix();
const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()).finished();
if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
if (H2) *H2 = R; // R
const Matrix2 R = r_.matrix();
Matrix21 Drotate1;
Drotate1 << -q.y(), q.x();
if (H1) *H1 << R, Drotate1;
if (H2) *H2 = R; // R
}
return q + t_;
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2) 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);
return Pose2(R,t);
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> 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 = I3;
return Pose2(R,t);
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> 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 = (Matrix(3, 3) <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0).finished();
}
if (H2) *H2 = I3;
return Pose2(R,t);
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point2 d = transform_to(point, H1, H2);
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {
// make temporary matrices
Matrix23 D1; Matrix2 D2;
Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version
if (!H1 && !H2) return Rot2::relativeBearing(d);
Matrix D_result_d;
Matrix12 D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * (*H1);
if (H2) *H2 = D_result_d * (*H2);
if (H1) *H1 = D_result_d * (D1);
if (H2) *H2 = D_result_d * (D2);
return result;
}
/* ************************************************************************* */
Rot2 Pose2::bearing(const Pose2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Rot2 result = bearing(point.t(), H1, H2);
Rot2 Pose2::bearing(const Pose2& pose,
OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const {
Matrix12 D2;
Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
if (H2) {
Matrix H2_ = *H2 * point.r().matrix();
*H2 = zeros(1, 3);
insertSub(*H2, H2_, 0, 0);
Matrix12 H2_ = D2 * pose.r().matrix();
*H2 << H2_, Z_1x1;
}
return result;
}
/* ************************************************************************* */
double Pose2::range(const Point2& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const {
Point2 d = point - t_;
if (!H1 && !H2) return d.norm();
Matrix H;
Matrix12 H;
double r = d.norm(H);
if (H1) *H1 = H * (Matrix(2, 3) <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0).finished();
if (H1) {
Matrix23 H1_;
H1_ << -r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0;
*H1 = H * H1_;
}
if (H2) *H2 = H;
return r;
}
/* ************************************************************************* */
double Pose2::range(const Pose2& pose2,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point2 d = pose2.t() - t_;
double Pose2::range(const Pose2& pose,
OptionalJacobian<1,3> H1,
OptionalJacobian<1,3> H2) const {
Point2 d = pose.t() - t_;
if (!H1 && !H2) return d.norm();
Matrix H;
Matrix12 H;
double r = d.norm(H);
if (H1) *H1 = H * (Matrix(2, 3) <<
if (H1) {
Matrix23 H1_;
H1_ <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0).finished();
if (H2) *H2 = H * (Matrix(2, 3) <<
pose2.r_.c(), -pose2.r_.s(), 0.0,
pose2.r_.s(), pose2.r_.c(), 0.0).finished();
-r_.s(), -r_.c(), 0.0;
*H1 = H * H1_;
}
if (H2) {
Matrix23 H2_;
H2_ <<
pose.r_.c(), -pose.r_.s(), 0.0,
pose.r_.s(), pose.r_.c(), 0.0;
*H2 = H * H2_;
}
return r;
}

131
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,72 +105,39 @@ public:
/// identity for group operation
inline static Pose2 identity() { return Pose2(); }
/// inverse transformation with derivatives
Pose2 inverse(boost::optional<Matrix&> H1=boost::none) const;
/// compose this transformation onto another (first *this and then p2)
Pose2 compose(const Pose2& p2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) 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
Vector 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 Vector Logmap(const Pose2& p);
static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
/**
* Calculate Adjoint map
* Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
*/
Matrix AdjointMap() const;
Matrix3 AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const {
assert(xi.size() == 3);
return AdjointMap()*xi;
}
/**
* Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
*/
static Matrix3 adjointMap(const Vector& v);
/**
* wedge for SE(2):
* @param xi 3-dim twist (v,omega) where
@ -179,34 +145,41 @@ public:
* v (vx,vy) = 2D velocity
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/
static inline Matrix wedge(double vx, double vy, double w) {
return (Matrix(3,3) <<
0.,-w, vx,
w, 0., vy,
0., 0., 0.).finished();
static inline Matrix3 wedge(double vx, double vy, double w) {
Matrix3 m;
m << 0.,-w, vx,
w, 0., vy,
0., 0., 0.;
return m;
}
/// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& v);
/// Derivative of Logmap
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
/// @{
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point) const;
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point,
boost::optional<Matrix23&> H1,
boost::optional<Matrix2&> H2) const;
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
OptionalJacobian<2, 3> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/** Return point coordinates in global frame */
Point2 transform_from(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<2, 3> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/** syntactic sugar for transform_from */
inline Point2 operator*(const Point2& point) const { return transform_from(point);}
@ -237,7 +210,7 @@ public:
inline const Rot2& rotation() const { return r_; }
//// return transformation matrix
Matrix matrix() const;
Matrix3 matrix() const;
/**
* Calculate bearing to a landmark
@ -245,17 +218,15 @@ public:
* @return 2D rotation \f$ \in SO(2) \f$
*/
Rot2 bearing(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
/**
* Calculate bearing to another pose
* @param point SO(2) location of other pose
* @return 2D rotation \f$ \in SO(2) \f$
*/
Rot2 bearing(const Pose2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
Rot2 bearing(const Pose2& pose,
OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
/**
* Calculate range to a landmark
@ -263,8 +234,8 @@ public:
* @return range (double)
*/
double range(const Point2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 2> H2=boost::none) const;
/**
* Calculate range to another pose
@ -272,8 +243,8 @@ public:
* @return range (double)
*/
double range(const Pose2& point,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1, 3> H1=boost::none,
OptionalJacobian<1, 3> H2=boost::none) const;
/// @}
/// @name Advanced Interface
@ -319,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

310
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,21 +27,21 @@ using namespace std;
namespace gtsam {
/** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Pose3);
/** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3);
static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3;
static const Matrix6 I6 = eye(6);
/* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) :
R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
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)
@ -50,69 +51,52 @@ Matrix6 Pose3::AdjointMap() const {
const Vector3 t = t_.vector();
Matrix3 A = skewSymmetric(t) * R;
Matrix6 adj;
adj << R, Z3, A, R;
adj << R, Z_3x3, A, R;
return adj;
}
/* ************************************************************************* */
Matrix6 Pose3::adjointMap(const Vector& xi) {
Matrix6 Pose3::adjointMap(const Vector6& xi) {
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
Matrix6 adj;
adj << w_hat, Z3, v_hat, w_hat;
adj << w_hat, Z_3x3, v_hat, w_hat;
return adj;
}
/* ************************************************************************* */
Vector Pose3::adjoint(const Vector& xi, const Vector& y,
boost::optional<Matrix&> H) {
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6,6> H) {
if (H) {
*H = zeros(6, 6);
H->setZero();
for (int i = 0; i < 6; ++i) {
Vector dxi = zero(6);
Vector6 dxi;
dxi.setZero();
dxi(i) = 1.0;
Matrix Gi = adjointMap(dxi);
(*H).col(i) = Gi * y;
Matrix6 Gi = adjointMap(dxi);
H->col(i) = Gi * y;
}
}
return adjointMap(xi) * y;
}
/* ************************************************************************* */
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
boost::optional<Matrix&> H) {
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6,6> H) {
if (H) {
*H = zeros(6, 6);
H->setZero();
for (int i = 0; i < 6; ++i) {
Vector dxi = zero(6);
Vector6 dxi;
dxi.setZero();
dxi(i) = 1.0;
Matrix GTi = adjointMap(dxi).transpose();
(*H).col(i) = GTi * y;
Matrix6 GTi = adjointMap(dxi).transpose();
H->col(i) = GTi * y;
}
}
Matrix adjT = adjointMap(xi).transpose();
return adjointMap(xi).transpose() * y;
}
/* ************************************************************************* */
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
// Bernoulli numbers, from Wikipedia
static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished();
static const int N = 5; // order of approximation
Matrix res = I6;
Matrix6 ad_i = I6;
Matrix6 ad_xi = adjointMap(xi);
double fac = 1.0;
for (int i = 1; i < N; ++i) {
ad_i = ad_xi * ad_i;
fac = fac * i;
res = res + B(i) / fac * ad_i;
}
return res;
}
/* ************************************************************************* */
void Pose3::print(const string& s) const {
cout << s;
@ -127,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));
@ -147,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) {
@ -168,64 +154,111 @@ 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);
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
}
/* ************************************************************************* */
// 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());
/**
* Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix
* J(xi) = [J_(w) Z_3x3;
* Q J_(w)]
* where J_(w) is the SO3 Expmap derivative.
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is similar to formula 102 in Barfoot14tro)
*/
static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
Vector3 w(sub(xi, 0, 3));
Vector3 v(sub(xi, 3, 6));
Matrix3 V = skewSymmetric(v);
Matrix3 W = skewSymmetric(w);
// Incorrect version
// Independently computes the logmap of the translation and rotation
// Vector v = t_.localCoordinates(T.translation());
Matrix3 Q;
// 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);
#ifdef NUMERICAL_EXPMAP_DERIV
Matrix3 Qj = Z_3x3;
double invFac = 1.0;
Q = Z_3x3;
Matrix3 Wj = I_3x3;
for (size_t j=1; j<10; ++j) {
Qj = Qj*W + Wj*V;
invFac = -invFac/(j+1);
Q = Q + invFac*Qj;
Wj = Wj*W;
}
#else
// The closed-form formula in Barfoot14tro eq. (102)
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;
}
/* ************************************************************************* */
Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) {
Vector3 w(sub(xi, 0, 3));
Matrix3 Jw = Rot3::ExpmapDerivative(w);
Matrix3 Q = computeQforExpmapDerivative(xi);
Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished();
return J;
}
/* ************************************************************************* */
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);
Matrix3 Q2 = -Jw*Q*Jw;
Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished();
return J;
}
/* ************************************************************************* */
Matrix4 Pose3::matrix() const {
const Matrix3 R = R_.matrix();
const Vector3 T = t_.vector();
Eigen::Matrix<double, 1, 4> A14;
Matrix14 A14;
A14 << 0.0, 0.0, 0.0, 1.0;
Matrix4 mat;
mat << R, T, A14;
@ -240,12 +273,11 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
}
/* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const {
Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
if (Dpose) {
const Matrix3 R = R_.matrix();
Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Dpose->resize(3, 6);
(*Dpose) << DR, R;
}
if (Dpoint)
@ -254,13 +286,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p) const {
return R_.unrotate(p - t_);
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
boost::optional<Matrix3&> Dpoint) const {
Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
OptionalJacobian<3,3> Dpoint) const {
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();
@ -278,77 +305,32 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const {
const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector());
if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z();
Dpose->resize(3, 6);
(*Dpose) <<
0.0, -wz, +wy,-1.0, 0.0, 0.0,
+wz, 0.0, -wx, 0.0,-1.0, 0.0,
-wy, +wx, 0.0, 0.0, 0.0,-1.0;
}
if (Dpoint)
*Dpoint = Rt;
return q;
}
/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1)
*H1 = p2.inverse().AdjointMap();
if (H2)
*H2 = I6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> 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, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Pose3 result = inverse() * p2;
if (H1)
*H1 = -result.inverse().AdjointMap();
if (H2)
*H2 = I6;
return result;
}
/* ************************************************************************* */
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
OptionalJacobian<1, 3> H2) const {
if (!H1 && !H2)
return transform_to(point).norm();
Point3 d = transform_to(point, H1, H2);
double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
d2);
Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished();
if (H1)
*H1 = D_result_d * (*H1);
if (H2)
*H2 = D_result_d * (*H2);
return n;
else {
Matrix36 D1;
Matrix3 D2;
Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z,
n = sqrt(d2);
Matrix13 D_result_d;
D_result_d << x / n, y / n, z / n;
if (H1) *H1 = D_result_d * D1;
if (H2) *H2 = D_result_d * D2;
return n;
}
}
/* ************************************************************************* */
double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
double r = range(point.translation(), H1, H2);
double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
OptionalJacobian<1,6> H2) const {
Matrix13 D2;
double r = range(pose.translation(), H1, H2? &D2 : 0);
if (H2) {
Matrix H2_ = *H2 * point.rotation().matrix();
*H2 = zeros(1, 6);
insertSub(*H2, H2_, 0, 3);
Matrix13 H2_ = D2 * pose.rotation().matrix();
*H2 << Matrix13::Zero(), H2_;
}
return r;
}
@ -370,7 +352,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
cq *= f;
// Add to form H matrix
Matrix H = zeros(3, 3);
Matrix3 H = Eigen::Matrix3d::Zero();
BOOST_FOREACH(const Point3Pair& pair, pairs){
Vector dp = pair.first.vector() - cp;
Vector dq = pair.second.vector() - cq;
@ -378,13 +360,13 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
}
// Compute SVD
Matrix U, V;
Matrix U,V;
Vector S;
svd(H, U, S, V);
// Recover transform with correction from Eggert97machinevisionandapplications
Matrix UVtranspose = U * V.transpose();
Matrix detWeighting = eye(3, 3);
Matrix3 UVtranspose = U * V.transpose();
Matrix3 detWeighting = I_3x3;
detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp);

450
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:
@ -70,6 +64,11 @@ public:
R_(R), t_(t) {
}
/** Construct from R,t, where t \in vector3 */
Pose3(const Rot3& R, const Vector3& t) :
R_(R), t_(t) {
}
/** Construct from Pose2 */
explicit Pose3(const Pose2& pose2);
@ -99,247 +98,218 @@ public:
}
/// inverse transformation with derivatives
Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const;
///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> 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 Vector& xi);
/**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/
static Vector adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
/**
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/
static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
/**
* Compute the inverse right-trivialized tangent (derivative) map of the exponential map,
* as detailed in [Kobilarov09siggraph] eq. (15)
* The full formula is documented in [Celledoni99cmame]
* Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and
* time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421-438, 2003.
* and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2
* Ernst Hairer, et al., Geometric Numerical Integration,
* Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006.
*/
static Matrix6 dExpInv_exp(const Vector& 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,
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> 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) const;
Point3 transform_to(const Point3& p,
boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) 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,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Calculate range to another pose
* @param pose Other SO(3) pose
* @return range (double)
*/
double range(const Pose3& pose,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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));
@ -352,21 +322,7 @@ 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{
};
template<>
struct GTSAM_EXPORT is_manifold<Pose3> : public boost::true_type{
};
template<>
struct GTSAM_EXPORT dimension<Pose3> : public boost::integral_constant<int, 6>{
};
}
struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
} // 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

52
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) {
@ -64,21 +60,43 @@ Rot2& Rot2::normalize() {
}
/* ************************************************************************* */
Matrix Rot2::matrix() const {
return (Matrix(2, 2) << c_, -s_, s_, c_).finished();
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));
}
/* ************************************************************************* */
Matrix Rot2::transpose() const {
return (Matrix(2, 2) << c_, s_, -s_, c_).finished();
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_;
rvalue_ << c_, -s_, s_, c_;
return rvalue_;
}
/* ************************************************************************* */
Matrix2 Rot2::transpose() const {
Matrix2 rvalue_;
rvalue_ << c_, s_, -s_, c_;
return rvalue_;
}
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1,
OptionalJacobian<2, 2> H2) const {
const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()).finished();
if (H1) *H1 << -q.y(), q.x();
if (H2) *H2 = matrix();
return q;
}
@ -86,21 +104,23 @@ Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
/* ************************************************************************* */
// see doc/math.lyx, SO(2) section
Point2 Rot2::unrotate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const {
const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished(); // R_{pi/2}q
if (H1) *H1 << q.y(), -q.x();
if (H2) *H2 = transpose();
return q;
}
/* ************************************************************************* */
Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if(fabs(n) > 1e-5) {
if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished();
if (H) {
*H << -y / d2, x / d2;
}
return Rot2::fromCosSin(x / n, y / n);
} else {
if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished();
if (H) *H << 0.0, 0.0;
return Rot2();
}
}

131
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) {
@ -87,7 +73,7 @@ namespace gtsam {
* @param H optional reference for Jacobian
* @return 2D rotation \f$ \in SO(2) \f$
*/
static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H =
static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
boost::none);
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
@ -111,68 +97,48 @@ 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, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = eye(1);
if (H2) *H2 = eye(1);
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, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
if (H1) *H1 = -eye(1);
if (H2) *H2 = eye(1);
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 Vector 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 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) {
return ones(1);
}
///Log map at identity - return the canonical coordinates of this rotation
static inline Vector Logmap(const Rot2& r) {
return (Vector(1) << r.theta()).finished();
/// Left-trivialized derivative inverse of the exponential map
static Matrix LogmapDerivative(const Vector& v) {
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
/// @{
@ -180,8 +146,8 @@ namespace gtsam {
/**
* rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
*/
Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/** syntactic sugar for rotate */
inline Point2 operator*(const Point2& p) const {
@ -191,8 +157,8 @@ namespace gtsam {
/**
* rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
*/
Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
OptionalJacobian<2, 2> H2 = boost::none) const;
/// @}
/// @name Standard Interface
@ -225,10 +191,10 @@ namespace gtsam {
}
/** return 2*2 rotation matrix */
Matrix matrix() const;
Matrix2 matrix() const;
/** return 2*2 transpose (inverse) rotation matrix */
Matrix transpose() const;
Matrix2 transpose() const;
private:
/** Serialization function */
@ -241,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

159
gtsam/geometry/Rot3.cpp
View File

@ -27,8 +27,6 @@ using namespace std;
namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
void Rot3::print(const std::string& s) const {
gtsam::print((Matrix)matrix(), s);
@ -54,7 +52,7 @@ Rot3 Rot3::Random(boost::mt19937 & rng) {
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w) {
Rot3 Rot3::rodriguez(const Vector3& w) {
double t = w.norm();
if (t < 1e-10) return Rot3();
return rodriguez(w/t, t);
@ -72,23 +70,21 @@ Point3 Rot3::operator*(const Point3& p) const {
/* ************************************************************************* */
Unit3 Rot3::rotate(const Unit3& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Unit3 q = Unit3(rotate(p.point3(Hp)));
if (Hp)
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
if (HR)
(*HR) = -q.basis().transpose() * matrix() * p.skew();
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
if (Hp) *Hp = q.basis().transpose() * matrix() * Dp;
if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
return q;
}
/* ************************************************************************* */
Unit3 Rot3::unrotate(const Unit3& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Unit3 q = Unit3(unrotate(p.point3(Hp)));
if (Hp)
(*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
if (HR)
(*HR) = q.basis().transpose() * q.skew();
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(unrotate(p.point3(Dp)));
if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
if (HR) *HR = q.basis().transpose() * q.skew();
return q;
}
@ -99,8 +95,8 @@ Unit3 Rot3::operator*(const Unit3& p) const {
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const {
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
@ -111,48 +107,6 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1) {
H1->resize(3,3);
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
}
if (H2)
*H2 = Rt;
return q;
}
/* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
return res;
}
/* ************************************************************************* */
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpInvL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
Matrix res = eye(3) + 0.5*x - s2*x2;
return res;
}
/* ************************************************************************* */
Point3 Rot3::column(int index) const{
if(index == 3)
@ -167,7 +121,7 @@ Point3 Rot3::column(int index) const{
/* ************************************************************************* */
Vector3 Rot3::xyz() const {
Matrix I;Vector3 q;
Matrix3 I;Vector3 q;
boost::tie(I,q)=RQ(matrix());
return q;
}
@ -195,36 +149,57 @@ Vector Rot3::quaternion() const {
}
/* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jr;
if (normx < 10e-8){
Jr = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
}
return Jr;
Matrix3 Rot3::ExpmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3;
double theta = x.norm(); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(x);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
return I_3x3 - 0.5*s1*s1*X + s2*X2;
#else // Luca's version
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
* where Jr = ExpmapDerivative(thetahat);
* This maps a perturbation in the tangent space (omega) to
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = x^, normalized by normx
const Matrix3 Y = skewSymmetric(x) / theta;
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
#endif
}
/* ************************************************************************* */
Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) {
// x is the axis-angle representation (exponential coordinates) for a rotation
double normx = norm_2(x); // rotation angle
Matrix3 Jrinv;
if (normx < 10e-8){
Jrinv = Matrix3::Identity();
}
else{
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
Jrinv = Matrix3::Identity() +
0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
}
return Jrinv;
Matrix3 Rot3::LogmapDerivative(const Vector3& x) {
if(zero(x)) return I_3x3;
double theta = x.norm();
#ifdef DUY_VERSION
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(x);
Matrix3 X2 = X*X;
double vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
return I_3x3 + 0.5*X - s2*X2;
#else // Luca's version
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
* where Jrinv = LogmapDerivative(omega);
* This maps a perturbation on the manifold (expmap(omega))
* to a perturbation in the tangent space (Jrinv * omega)
*/
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
return I_3x3 + 0.5 * X
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
* X;
#endif
}
/* ************************************************************************* */
@ -255,18 +230,12 @@ ostream &operator<<(ostream &os, const Rot3& R) {
return os;
}
/* ************************************************************************* */
Point3 Rot3::unrotate(const Point3& p) const {
// Eigen expression
return Point3(transpose()*p.vector()); // q = Rt*p
}
/* ************************************************************************* */
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));
}
/* ************************************************************************* */

154
gtsam/geometry/Rot3.h
View File

@ -16,11 +16,15 @@
* @author Christian Potthast
* @author Frank Dellaert
* @author Richard Roberts
* @author Luca Carlone
*/
// \callgraph
#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
@ -39,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
@ -58,7 +52,7 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Rot3 {
class GTSAM_EXPORT Rot3 : public LieGroup<Rot3,3> {
private:
@ -157,7 +151,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
@ -180,7 +174,7 @@ namespace gtsam {
* @param v a vector of incremental roll,pitch,yaw
* @return incremental rotation matrix
*/
static Rot3 rodriguez(const Vector& v);
static Rot3 rodriguez(const Vector3& v);
/**
* Rodriguez' formula to compute an incremental rotation matrix
@ -190,7 +184,7 @@ namespace gtsam {
* @return incremental rotation matrix
*/
static Rot3 rodriguez(double wx, double wy, double wz)
{ return rodriguez((Vector(3) << wx, wy, wz).finished());}
{ return rodriguez(Vector3(wx, wy, wz));}
/// @}
/// @name Testable
@ -211,23 +205,13 @@ namespace gtsam {
return Rot3();
}
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) 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
@ -238,23 +222,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,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> 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
@ -268,21 +239,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
/// @{
@ -291,32 +270,33 @@ namespace gtsam {
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula
*/
static Rot3 Expmap(const Vector& v) {
if(zero(v)) return Rot3();
else return rodriguez(v);
static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) {
if(H) *H = Rot3::ExpmapDerivative(v);
if (zero(v)) return Rot3(); else return rodriguez(v);
}
/**
* Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
static Vector3 Logmap(const Rot3& R);
static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none);
/// Left-trivialized derivative of the exponential map
static Matrix3 dexpL(const Vector3& v);
/// Derivative of Expmap
static Matrix3 ExpmapDerivative(const Vector3& x);
/// Left-trivialized derivative inverse of the exponential map
static Matrix3 dexpInvL(const Vector3& v);
/// Derivative of Logmap
static Matrix3 LogmapDerivative(const Vector3& x);
/**
* Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
*/
static Matrix3 rightJacobianExpMapSO3(const Vector3& x);
/** Calculate Adjoint map */
Matrix3 AdjointMap() const { return matrix(); }
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
*/
static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x);
// 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
@ -325,34 +305,27 @@ namespace gtsam {
/**
* rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
*/
Point3 rotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
/// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
/// @}
/// @name Group Action on Unit3
/// @{
/// rotate 3D direction from rotated coordinate frame to world frame
Unit3 rotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
/// unrotate 3D direction from world frame to rotated coordinate frame
Unit3 unrotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
/// rotate 3D direction from rotated coordinate frame to world frame
Unit3 operator*(const Unit3& p) const;
@ -485,20 +458,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> {};
}

128
gtsam/geometry/Rot3M.cpp
View File

@ -30,10 +30,8 @@ using namespace std;
namespace gtsam {
static const Matrix3 I3 = Matrix3::Identity();
/* ************************************************************************* */
Rot3::Rot3() : rot_(Matrix3::Identity()) {}
Rot3::Rot3() : rot_(I_3x3) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
@ -112,7 +110,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;
@ -134,27 +132,6 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
-swy + C02, swx + C12, c + C22);
}
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2) const {
return *this * R2;
}
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return *this * R2;
}
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return *this * R2;
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_));
@ -166,24 +143,9 @@ Matrix3 Rot3::transpose() const {
return rot_.transpose();
}
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -rot_;
return Rot3(Matrix3(transpose()));
}
/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I3;
Matrix3 R12 = transpose()*R2.rot_;
return Rot3(R12);
}
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1 || H2) {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
@ -193,7 +155,7 @@ Point3 Rot3::rotate(const Point3& p,
/* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) {
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
static const double PI = boost::math::constants::pi<double>();
@ -201,6 +163,8 @@ Vector3 Rot3::Logmap(const Rot3& R) {
// Get trace(R)
double tr = rot.trace();
Vector3 thetaR;
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special
if (std::abs(tr+1.0) < 1e-10) {
@ -211,7 +175,7 @@ Vector3 Rot3::Logmap(const Rot3& R) {
return (PI / sqrt(2.0+2.0*rot(1,1))) *
Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1));
else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
return (PI / sqrt(2.0+2.0*rot(0,0))) *
thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) *
Vector3(1.0+rot(0,0), rot(1,0), rot(2,0));
} else {
double magnitude;
@ -224,68 +188,60 @@ Vector3 Rot3::Logmap(const Rot3& R) {
// use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2)
magnitude = 0.5 - tr_3*tr_3/12.0;
}
return magnitude*Vector3(
thetaR = magnitude*Vector3(
rot(2,1)-rot(1,2),
rot(0,2)-rot(2,0),
rot(1,0)-rot(0,1));
}
if(H) *H = Rot3::LogmapDerivative(thetaR);
return thetaR;
}
/* ************************************************************************* */
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) {
Matrix 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");
}
/* ************************************************************************* */

54
gtsam/geometry/Rot3Q.cpp
View File

@ -80,25 +80,13 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
return Quaternion(Eigen::AngleAxisd(theta, w)); }
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_);
Rot3 Rot3::rodriguez(const Vector3& w, double theta) {
return QuaternionChart::Expmap(theta,w);
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_);
@ -110,7 +98,7 @@ namespace gtsam {
}
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -matrix();
return Rot3(quaternion_.inverse());
}
@ -125,15 +113,15 @@ namespace gtsam {
/* ************************************************************************* */
Rot3 Rot3::between(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
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;
}
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
Matrix R = matrix();
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R;
@ -142,31 +130,9 @@ namespace gtsam {
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R) {
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;
const Quaternion& q = R.quaternion_;
const double qw = q.w();
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
return (2 - 2 * (qw - 1) / 3) * q.vec();
} else if (qw < NearlyNegativeOne) {
// Angle is zero, return zero vector
return 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;
return (angle / s) * q.vec();
}
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
if(H) *H = Rot3::LogmapDerivative(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

14
gtsam/geometry/SimpleCamera.cpp
View File

@ -21,27 +21,27 @@
namespace gtsam {
SimpleCamera simpleCamera(const Matrix& P) {
SimpleCamera simpleCamera(const Matrix34& P) {
// P = [A|a] = s K cRw [I|-T], with s the unknown scale
Matrix A = P.topLeftCorner(3, 3);
Vector a = P.col(3);
Matrix3 A = P.topLeftCorner(3, 3);
Vector3 a = P.col(3);
// do RQ decomposition to get s*K and cRw angles
Matrix sK;
Vector xyz;
Matrix3 sK;
Vector3 xyz;
boost::tie(sK, xyz) = RQ(A);
// Recover scale factor s and K
double s = sK(2, 2);
Matrix K = sK / s;
Matrix3 K = sK / s;
// Recover cRw itself, and its inverse
Rot3 cRw = Rot3::RzRyRx(xyz);
Rot3 wRc = cRw.inverse();
// Now, recover T from a = - s K cRw T = - A T
Vector T = -(A.inverse() * a);
Vector3 T = -(A.inverse() * a);
return SimpleCamera(Pose3(wRc, T),
Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
}

2
gtsam/geometry/SimpleCamera.h
View File

@ -27,5 +27,5 @@ namespace gtsam {
typedef PinholeCamera<Cal3_S2> SimpleCamera;
/// Recover camera from 3*4 camera matrix
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix& P);
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
}

36
gtsam/geometry/StereoCamera.cpp
View File

@ -30,8 +30,8 @@ namespace gtsam {
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
OptionalJacobian<3,6> H3) const {
#ifdef STEREOCAMERA_CHAIN_RULE
const Point3 q = leftCamPose_.transform_to(point, H1, H2);
@ -58,28 +58,28 @@ namespace gtsam {
if (H1 || H2) {
#ifdef STEREOCAMERA_CHAIN_RULE
// just implement chain rule
Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
if (H1) *H1 = D_project_point*(*H1);
if (H2) *H2 = D_project_point*(*H2);
#else
// optimized version, see StereoCamera.nb
if (H1) {
const double v1 = v/fy, v2 = fx*v1, dx=d*x;
*H1 = (Matrix(3, 6) <<
uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
*H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR,
fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v
).finished();
fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v;
}
if (H2) {
const Matrix R(leftCamPose_.rotation().matrix());
*H2 = d * (Matrix(3, 3) <<
fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v
).finished();
const Matrix3 R(leftCamPose_.rotation().matrix());
*H2 << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v;
*H2 << d * (*H2);
}
#endif
if (H3)
// TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet
H3->setZero();
}
// finally translate
@ -87,15 +87,15 @@ namespace gtsam {
}
/* ************************************************************************* */
Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
double d = 1.0 / P.z(), d2 = d*d;
const Cal3_S2Stereo& K = *K_;
double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
return (Matrix(3, 3) <<
f_x*d, 0.0, -d2*f_x* P.x(),
Matrix3 m;
m << f_x*d, 0.0, -d2*f_x* P.x(),
f_x*d, 0.0, -d2*f_x*(P.x() - b),
0.0, f_y*d, -d2*f_y* P.y()
).finished();
0.0, f_y*d, -d2*f_y* P.y();
return m;
}
}

37
gtsam/geometry/StereoCamera.h
View File

@ -44,6 +44,8 @@ private:
public:
enum { dimension = 6 };
/// @name Standard Constructors
/// @{
@ -90,14 +92,8 @@ public:
}
/// Local coordinates of manifold neighborhood around current value
inline Vector localCoordinates(const StereoCamera& t2) const {
return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_));
}
Pose3 between(const StereoCamera &camera,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
return leftCamPose_.between(camera.pose(), H1, H2);
inline Vector6 localCoordinates(const StereoCamera& t2) const {
return leftCamPose_.localCoordinates(t2.leftCamPose_);
}
/// @}
@ -119,9 +115,9 @@ public:
* @param H3 IGNORED (for calibration)
*/
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const;
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none,
OptionalJacobian<3, 6> H3 = boost::none) const;
/**
*
@ -139,7 +135,7 @@ public:
private:
/** utility functions */
Matrix Dproject_to_stereo_camera1(const Point3& P) const;
Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
friend class boost::serialization::access;
template<class Archive>
@ -150,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> {};
}

43
gtsam/geometry/Unit3.cpp
View File

@ -39,14 +39,13 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
Unit3 direction(point);
if (H) {
// 3*3 Derivative of representation with respect to point is 3*3:
Matrix D_p_point;
Matrix3 D_p_point;
point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
// Calculate the 2*3 Jacobian
H->resize(2, 3);
*H << direction.basis().transpose() * D_p_point;
}
return direction;
@ -67,7 +66,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
}
/* ************************************************************************* */
const Unit3::Matrix32& Unit3::basis() const {
const Matrix32& Unit3::basis() const {
// Return cached version if exists
if (B_)
@ -92,7 +91,7 @@ const Unit3::Matrix32& Unit3::basis() const {
b2 = b2 / b2.norm();
// Create the basis matrix
B_.reset(Unit3::Matrix32());
B_.reset(Matrix32());
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
return *B_;
}
@ -104,38 +103,39 @@ void Unit3::print(const std::string& s) const {
}
/* ************************************************************************* */
Matrix Unit3::skew() const {
Matrix3 Unit3::skew() const {
return skewSymmetric(p_.x(), p_.y(), p_.z());
}
/* ************************************************************************* */
Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
// 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
Matrix Bt = basis().transpose();
Vector xi = Bt * q.p_.vector();
Matrix23 Bt = basis().transpose();
Vector2 xi = Bt * q.p_.vector();
if (H)
*H = Bt * q.basis();
return xi;
}
/* ************************************************************************* */
double Unit3::distance(const Unit3& q, boost::optional<Matrix&> H) const {
Vector xi = error(q, H);
double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
Matrix2 H_;
Vector2 xi = error(q, H_);
double theta = xi.norm();
if (H)
*H = (xi.transpose() / theta) * (*H);
*H = (xi.transpose() / theta) * H_;
return theta;
}
/* ************************************************************************* */
Unit3 Unit3::retract(const Vector& v) const {
Unit3 Unit3::retract(const Vector2& v) const {
// Get the vector form of the point and the basis matrix
Vector p = Point3::Logmap(p_);
Matrix B = basis();
Vector3 p = p_.vector();
Matrix32 B = basis();
// Compute the 3D xi_hat vector
Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
double xi_hat_norm = xi_hat.norm();
@ -147,28 +147,27 @@ Unit3 Unit3::retract(const Vector& v) const {
return Unit3(-point3());
}
Vector exp_p_xi_hat = cos(xi_hat_norm) * p
Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
+ sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
return Unit3(exp_p_xi_hat);
}
/* ************************************************************************* */
Vector Unit3::localCoordinates(const Unit3& y) const {
Vector2 Unit3::localCoordinates(const Unit3& y) const {
Vector p = Point3::Logmap(p_);
Vector q = Point3::Logmap(y.p_);
Vector3 p = p_.vector(), q = y.p_.vector();
double dot = p.dot(q);
// Check for special cases
if (std::abs(dot - 1.0) < 1e-16)
return Vector2(0, 0);
else if (std::abs(dot + 1.0) < 1e-16)
return (Vector(2) << M_PI, 0).finished();
return Vector2(M_PI, 0);
else {
// no special case
double theta = acos(dot);
Vector result_hat = (theta / sin(theta)) * (q - p * dot);
Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
return basis().transpose() * result_hat;
}
}

56
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,17 +29,17 @@
namespace gtsam {
/// Represents a 3D point on a unit sphere.
class GTSAM_EXPORT Unit3{
class GTSAM_EXPORT Unit3 {
private:
typedef Eigen::Matrix<double,3,2> Matrix32;
Point3 p_; ///< The location of the point on the unit sphere
mutable boost::optional<Matrix32> B_; ///< Cached basis
public:
enum { dimension = 2 };
/// @name Constructors
/// @{
@ -52,6 +53,11 @@ public:
p_(p / p.norm()) {
}
/// Construct from a vector3
explicit Unit3(const Vector3& p) :
p_(p / p.norm()) {
}
/// Construct from x,y,z
Unit3(double x, double y, double z) :
p_(x, y, z) {
@ -59,7 +65,7 @@ public:
}
/// Named constructor from Point3 with optional Jacobian
static Unit3 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H =
boost::none);
/// Random direction, using boost::uniform_on_sphere
@ -90,10 +96,10 @@ public:
const Matrix32& basis() const;
/// Return skew-symmetric associated with 3D point on unit sphere
Matrix skew() const;
Matrix3 skew() const;
/// Return unit-norm Point3
const Point3& point3(boost::optional<Matrix&> H = boost::none) const {
const Point3& point3(OptionalJacobian<3,2> H = boost::none) const {
if (H)
*H = basis();
return p_;
@ -105,12 +111,12 @@ public:
}
/// Signed, vector-valued error between two directions
Vector error(const Unit3& q,
boost::optional<Matrix&> H = boost::none) const;
Vector2 error(const Unit3& q,
OptionalJacobian<2,2> H = boost::none) const;
/// Distance between two directions
double distance(const Unit3& q,
boost::optional<Matrix&> H = boost::none) const;
OptionalJacobian<1,2> H = boost::none) const;
/// @}
@ -133,10 +139,10 @@ public:
};
/// The retract function
Unit3 retract(const Vector& v) const;
Unit3 retract(const Vector2& v) const;
/// The local coordinates function
Vector localCoordinates(const Unit3& s) const;
Vector2 localCoordinates(const Unit3& s) const;
/// @}
@ -144,13 +150,18 @@ private:
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(B_);
// homebrew serialize Eigen Matrix
ar & boost::serialization::make_nvp("B11", (*B_)(0,0));
ar & boost::serialization::make_nvp("B12", (*B_)(0,1));
ar & boost::serialization::make_nvp("B21", (*B_)(1,0));
ar & boost::serialization::make_nvp("B22", (*B_)(1,1));
ar & boost::serialization::make_nvp("B31", (*B_)(2,0));
ar & boost::serialization::make_nvp("B32", (*B_)(2,1));
}
/// @}
@ -158,24 +169,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

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

@ -60,7 +60,7 @@ TEST( Cal3_S2, calibrate_homogeneous) {
Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
TEST( Cal3_S2, Duncalibrate1)
{
Matrix computed; K.uncalibrate(p, computed, boost::none);
Matrix25 computed; K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-8));
}

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

@ -15,12 +15,14 @@
* @brief test CalibratedCamera class
*/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
using namespace std;
using 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);
}
//******************************************************************************

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

@ -15,29 +15,28 @@
* @brief test PinholeCamera class
*/
#include <cmath>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <cmath>
#include <iostream>
using namespace std;
using namespace gtsam;
typedef PinholeCamera<Cal3_S2> Camera;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.
).finished(),
Point3(0,0,0.5));
static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
static const Camera camera(pose, K);
typedef PinholeCamera<Cal3_S2> Camera;
static const Camera camera(pose1, K);
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
static const Camera camera1(pose1, K);
static const Point3 point1(-0.08,-0.08, 0.0);
static const Point3 point2(-0.08, 0.08, 0.0);
@ -52,8 +51,8 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0);
/* ************************************************************************* */
TEST( PinholeCamera, constructor)
{
CHECK(assert_equal( camera.calibration(), K));
CHECK(assert_equal( camera.pose(), pose1));
EXPECT(assert_equal( camera.calibration(), K));
EXPECT(assert_equal( camera.pose(), pose));
}
/* ************************************************************************* */
@ -67,7 +66,7 @@ TEST( PinholeCamera, level2)
Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
Rot3 wRc(x,y,z);
Pose3 expected(wRc,Point3(0.4,0.3,0.1));
CHECK(assert_equal( camera.pose(), expected));
EXPECT(assert_equal( camera.pose(), expected));
}
/* ************************************************************************* */
@ -80,72 +79,72 @@ TEST( PinholeCamera, lookat)
// expected
Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
Pose3 expected(Rot3(xc,yc,zc),C);
CHECK(assert_equal( camera.pose(), expected));
EXPECT(assert_equal( camera.pose(), expected));
Point3 C2(30.0,0.0,10.0);
Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
Matrix R = camera2.pose().rotation().matrix();
Matrix I = trans(R)*R;
CHECK(assert_equal(I, eye(3)));
EXPECT(assert_equal(I, eye(3)));
}
/* ************************************************************************* */
TEST( PinholeCamera, project)
{
CHECK(assert_equal( camera.project(point1), Point2(-100, 100) ));
CHECK(assert_equal( camera.project(point2), Point2(-100, -100) ));
CHECK(assert_equal( camera.project(point3), Point2( 100, -100) ));
CHECK(assert_equal( camera.project(point4), Point2( 100, 100) ));
EXPECT(assert_equal( camera.project(point1), Point2(-100, 100) ));
EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) ));
EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) ));
EXPECT(assert_equal( camera.project(point4), Point2( 100, 100) ));
}
/* ************************************************************************* */
TEST( PinholeCamera, backproject)
{
CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1));
EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2));
EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3));
EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4));
}
/* ************************************************************************* */
TEST( PinholeCamera, backprojectInfinity)
{
CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf));
CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf));
CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf));
CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf));
EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf));
}
/* ************************************************************************* */
TEST( PinholeCamera, backproject2)
{
Point3 origin;
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(Point2(), x.first));
CHECK(x.second);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x.first));
EXPECT(x.second);
}
/* ************************************************************************* */
TEST( PinholeCamera, backprojectInfinity2)
{
Point3 origin;
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backprojectPointAtInfinity(Point2());
Point3 expected(0., 1., 0.);
Point2 x = camera.projectPointAtInfinity(expected);
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(Point2(), x));
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
}
/* ************************************************************************* */
@ -159,8 +158,8 @@ TEST( PinholeCamera, backprojectInfinity3)
Point3 expected(0., 0., 1.);
Point2 x = camera.projectPointAtInfinity(expected);
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(Point2(), x));
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
}
/* ************************************************************************* */
@ -173,13 +172,13 @@ TEST( PinholeCamera, Dproject)
{
Matrix Dpose, Dpoint, Dcal;
Point2 result = camera.project(point1, Dpose, Dpoint, Dcal);
Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K);
Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K);
Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K);
CHECK(assert_equal(Point2(-100, 100), result));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K);
Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
Matrix numerical_cal = numericalDerivative33(project3, pose, point1, K);
EXPECT(assert_equal(Point2(-100, 100), result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
EXPECT(assert_equal(numerical_cal, Dcal, 1e-7));
}
/* ************************************************************************* */
@ -190,21 +189,21 @@ static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const C
TEST( PinholeCamera, Dproject_Infinity)
{
Matrix Dpose, Dpoint, Dcal;
Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera
Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
// test Projection
Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
Point2 expected(-5.0, 5.0);
CHECK(assert_equal(actual, expected, 1e-7));
EXPECT(assert_equal(actual, expected, 1e-7));
// test Jacobians
Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K);
Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K);
Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters
Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7));
CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K);
Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K);
Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters
Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K);
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7));
EXPECT(assert_equal(numerical_cal, Dcal, 1e-7));
}
/* ************************************************************************* */
@ -217,11 +216,80 @@ TEST( PinholeCamera, Dproject2)
{
Matrix Dcamera, Dpoint;
Point2 result = camera.project2(point1, Dcamera, Dpoint);
Matrix numerical_camera = numericalDerivative21(project4, camera, point1);
Matrix numerical_point = numericalDerivative22(project4, camera, point1);
CHECK(assert_equal(result, Point2(-100, 100) ));
CHECK(assert_equal(numerical_camera, Dcamera, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
Matrix Hexpected2 = numericalDerivative22(project4, camera, point1);
EXPECT(assert_equal(result, Point2(-100, 100) ));
EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
}
/* ************************************************************************* */
static double range0(const Camera& camera, const Point3& point) {
return camera.range(point);
}
/* ************************************************************************* */
TEST( PinholeCamera, range0) {
Matrix D1; Matrix D2;
double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
static double range1(const Camera& camera, const Pose3& pose) {
return camera.range(pose);
}
/* ************************************************************************* */
TEST( PinholeCamera, range1) {
Matrix D1; Matrix D2;
double result = camera.range(pose1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
typedef PinholeCamera<Cal3Bundler> Camera2;
static const Cal3Bundler K2(625, 1e-3, 1e-3);
static const Camera2 camera2(pose1, K2);
static double range2(const Camera& camera, const Camera2& camera2) {
return camera.range<Cal3Bundler>(camera2);
}
/* ************************************************************************* */
TEST( PinholeCamera, range2) {
Matrix D1; Matrix D2;
double result = camera.range<Cal3Bundler>(camera2, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */
static const CalibratedCamera camera3(pose1);
static double range3(const Camera& camera, const CalibratedCamera& camera3) {
return camera.range(camera3);
}
/* ************************************************************************* */
TEST( PinholeCamera, range3) {
Matrix D1; Matrix D2;
double result = camera.range(camera3, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));
}
/* ************************************************************************* */

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));
}
/* ************************************************************************* */

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