From 05a733956ed612c1273c914d06024fcacbb0fc82 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 16:04:06 +0100 Subject: [PATCH 001/199] Start of a proposed Concepts framework enabled by traits, addressing issue #180 --- GTSAM-Concepts.md | 87 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 87 insertions(+) create mode 100644 GTSAM-Concepts.md diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md new file mode 100644 index 000000000..6c09cc467 --- /dev/null +++ b/GTSAM-Concepts.md @@ -0,0 +1,87 @@ +GTSAM Concepts +============== + +Concepts define (see [Generic Programming Techniques](http://www.boost.org/community/generic_programming.html)) + +* associated types +* valid expressions +* invariants +* complexity guarantees + +GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the concepts MANIFOLD, GROUP, LIE_GROUP, VECTOR_SPACE + +`gtsam::traits` is our way to associate these concepts with types. We will not use Eigen-style or STL-style traits, that define many properties at once. Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming. + +Traits allow us to play with types that are outside GTSAM control, e.g., `Eigen::VectorXd`. + +The naming conventions are as follows: + +* Types: `gtsam::traits::SomeAssociatedType::type`, i.e., they are MixedCase and define a `type`, for example: + + template<> + gtsam::traits::TangentVector { + typedef Vector2 type; + } + +* Values: `gtsam::traits::someValue::value`, i.e., they are mixedCase starting with a lowercase letter and define a `value`, but also a `value_type`. For example: + + template<> + gtsam::traits::dimension { + static const int value = 2; + typedef const int value_type; // const ? + } + +* Functors: `gtsam::traits::someFunctor`, i.e., they are mixedCase starting with a lowercase letter and define a functor type + + template<> + gtsam::traits::retract { + Point2 operator()(const Point2& p, const Vector2& v) { + return Point2(p.x()+v[0], p.y()+v[1]); + } + } + + The above is still up in the air, can we not point to a function somewhere? Or a method? + +Concepts are associated with a tag. + +Manifold +-------- + +`gtsam::tags::manifold_tag` + +`gtsam::traits::structure_tag` + +* associated types: DefaultChart +* valid expressions: dimension + +Chart +----- + + +Group +----- + +Lie Group +--------- + +Vector Space +------------ + +Examples +-------- + +An example of implementing a Manifold is here: + + + namespace gtsam { + namespace traits { + + // types: + template + struct SomeAssociatedType::type + + } + } + + + From c7676f10121a20ff488a5ba195b4610a9a36b1d2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 16:41:11 +0100 Subject: [PATCH 002/199] More changes --- GTSAM-Concepts.md | 52 +++++++++++++++++++++++++++++++++++++---------- 1 file changed, 41 insertions(+), 11 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 6c09cc467..9b848bf51 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -10,6 +10,9 @@ Concepts define (see [Generic Programming Techniques](http://www.boost.org/commu GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the concepts MANIFOLD, GROUP, LIE_GROUP, VECTOR_SPACE +traits +------ + `gtsam::traits` is our way to associate these concepts with types. We will not use Eigen-style or STL-style traits, that define many properties at once. Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming. Traits allow us to play with types that are outside GTSAM control, e.g., `Eigen::VectorXd`. @@ -31,42 +34,69 @@ The naming conventions are as follows: typedef const int value_type; // const ? } -* Functors: `gtsam::traits::someFunctor`, i.e., they are mixedCase starting with a lowercase letter and define a functor type +* Functors: `gtsam::traits::someFunctor::type`, i.e., they are mixedCase starting with a lowercase letter and define a functor `type`. Example - template<> - gtsam::traits::retract { - Point2 operator()(const Point2& p, const Vector2& v) { - return Point2(p.x()+v[0], p.y()+v[1]); + struct Point2::retract { + Point2 p_; + retract(const Point2& p) : p_(p) {} + Point2 operator()(const Vector2& v) { + return Point2(p.x()+v[0], p.y()+v[1]); } } - The above is still up in the air, can we not point to a function somewhere? Or a method? + template<> + gtsam::traits::retract { + typedef Point2::retract type; + } + + The above is still up in the air. Do we need the type indirection? + +tags +---- Concepts are associated with a tag. +* `gtsam::tags::manifold_tag` +* `gtsam::tags::group_tag` +* `gtsam::tags::lie_group_tag` +* `gtsam::tags::vector_space_tag` + +Can be queried `gtsam::traits::structure_tag` + + Manifold -------- -`gtsam::tags::manifold_tag` +* types: `DefaultChart` +* values: `dimension` -`gtsam::traits::structure_tag` - -* associated types: DefaultChart -* valid expressions: dimension +Anything else? Chart ----- +* types: `Manifold`, `Vector` +* values: `retract`, `local` + +Are these values? They are just methods. Anything else? + Group ----- +* values: `identity` +* values: `compose`, `inverse`, (`between`) + Lie Group --------- +Implements both MANIFOLD and GROUP + Vector Space ------------ +Lie Group where compose == `+` + Examples -------- From 316bde9427d7c694e630276141d4da43610ebf0f Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 16:51:22 +0100 Subject: [PATCH 003/199] Example --- GTSAM-Concepts.md | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 9b848bf51..c22563168 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -102,14 +102,30 @@ Examples An example of implementing a Manifold is here: + // GTSAM type + class Rot2 { + ... + class Chart { + ... + } + } namespace gtsam { namespace traits { - // types: - template - struct SomeAssociatedType::type - + template<> + struct DefaultChart { + typedef Rot2::Chart type; + } + + template<> + struct Manifold { + typedef Rot2 type; + } + + template<> + struct Vector { + typedef Vector2 type; } } From 7b5ddbf21564cc7d4c5d67ab60cb775558e8a7d5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 17:46:00 +0100 Subject: [PATCH 004/199] Stuff about manifolds --- GTSAM-Concepts.md | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index c22563168..39bb4087d 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -34,9 +34,10 @@ The naming conventions are as follows: typedef const int value_type; // const ? } -* Functors: `gtsam::traits::someFunctor::type`, i.e., they are mixedCase starting with a lowercase letter and define a functor `type`. Example +* Functors: `gtsam::traits::someFunctor::type`, i.e., they are mixedCase starting with a lowercase letter and define a functor `type`. The funcor itself should define a `result_type`. Example struct Point2::retract { + typedef Point2 result_type; Point2 p_; retract(const Point2& p) : p_(p) {} Point2 operator()(const Vector2& v) { @@ -49,7 +50,13 @@ The naming conventions are as follows: typedef Point2::retract type; } - The above is still up in the air. Do we need the type indirection? + The above is still up in the air. Do we need the type indirection? Could we just inherit the trait like so + + template<> + gtsam::traits::retract : Point2::retract { + } + + In which case we could just say `gtsam::traits::retract(p)(v)`. tags ---- @@ -64,11 +71,24 @@ Concepts are associated with a tag. Can be queried `gtsam::traits::structure_tag` +Testable +-------- + +* values: `print`, `equals` + +Anything else? + Manifold -------- -* types: `DefaultChart` +[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 the vector space. + +In GTSAM we assume that a manifold type can yield such a chart at any point, and we require that a functor `defaultChart` is available that + * values: `dimension` +* functors `defaultChart` +* types: `DefaultChart` is the *type* of chart returned by the functor `defaultChart` +* invariants: `defaultChart::result_type == DefaultChart::type` Anything else? From c05f43ab511955dbc5f01f26b20fe2de39dc3659 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 17:49:43 +0100 Subject: [PATCH 005/199] Concepts first, implementation details later --- GTSAM-Concepts.md | 97 ++++++++++++++++++++++++----------------------- 1 file changed, 50 insertions(+), 47 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 39bb4087d..309635e1d 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -4,11 +4,59 @@ GTSAM Concepts Concepts define (see [Generic Programming Techniques](http://www.boost.org/community/generic_programming.html)) * associated types -* valid expressions +* valid expressions, like functions and values * invariants * complexity guarantees -GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the concepts MANIFOLD, GROUP, LIE_GROUP, VECTOR_SPACE +GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts. + +Testable +-------- +Unit tests heavily depend on the following two functions being defined for all types that need to be tested: + +* functions: `print`, `equals` + +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 the vector space. + +In GTSAM we assume that a manifold type can yield such a chart at any point, and we require that a functor `defaultChart` is available that + +* values: `dimension` +* functors `defaultChart` +* types: `DefaultChart` is the *type* of chart returned by the functor `defaultChart` +* invariants: `defaultChart::result_type == DefaultChart::type` + +Anything else? + +Chart +----- + +* types: `Manifold`, `Vector` +* values: `retract`, `local` + +Are these values? They are just methods. Anything else? + + +Group +----- + +* values: `identity` +* values: `compose`, `inverse`, (`between`) + +Lie Group +--------- + +Implements both MANIFOLD and GROUP + +Vector Space +------------ + +Lie Group where compose == `+` + traits ------ @@ -71,51 +119,6 @@ Concepts are associated with a tag. Can be queried `gtsam::traits::structure_tag` -Testable --------- - -* values: `print`, `equals` - -Anything else? - -Manifold --------- - -[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 the vector space. - -In GTSAM we assume that a manifold type can yield such a chart at any point, and we require that a functor `defaultChart` is available that - -* values: `dimension` -* functors `defaultChart` -* types: `DefaultChart` is the *type* of chart returned by the functor `defaultChart` -* invariants: `defaultChart::result_type == DefaultChart::type` - -Anything else? - -Chart ------ - -* types: `Manifold`, `Vector` -* values: `retract`, `local` - -Are these values? They are just methods. Anything else? - - -Group ------ - -* values: `identity` -* values: `compose`, `inverse`, (`between`) - -Lie Group ---------- - -Implements both MANIFOLD and GROUP - -Vector Space ------------- - -Lie Group where compose == `+` Examples -------- From 079d1b122dc3ca6cb797b04afff7ff79b2fab7ea Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 17:53:56 +0100 Subject: [PATCH 006/199] More manifold stuff --- GTSAM-Concepts.md | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 309635e1d..2f477f380 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -23,11 +23,13 @@ To optimize over continuous types, we assume they are manifolds. This is central [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 the vector space. -In GTSAM we assume that a manifold type can yield such a chart at any point, and we require that a functor `defaultChart` is available that +In GTSAM we assume that a manifold type can yield such a chart at any point, and we require that a functor `defaultChart` is available that, when called for any point on the manifold, returns a Chart type. * values: `dimension` -* functors `defaultChart` -* types: `DefaultChart` is the *type* of chart returned by the functor `defaultChart` +* types: `Vector`, type that lives in tangent space + * + * `DefaultChart` is the *type* of the chart returned by the functor `defaultChart` +* functor `defaultChart`, returns a `DefaultChart` * invariants: `defaultChart::result_type == DefaultChart::type` Anything else? @@ -35,7 +37,7 @@ Anything else? Chart ----- -* types: `Manifold`, `Vector` +* types: `Manifold`, a pointer back to the type * values: `retract`, `local` Are these values? They are just methods. Anything else? From 68422161a249f7ec5b559a9be1f653f41bf4c77b Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 18:24:49 +0100 Subject: [PATCH 007/199] Moved Testable, not so important --- GTSAM-Concepts.md | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 2f477f380..4540a0fb6 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -1,20 +1,15 @@ GTSAM Concepts ============== -Concepts define (see [Generic Programming Techniques](http://www.boost.org/community/generic_programming.html)) +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 -GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts. +Below we discuss the most important concepts use in GTSAM, and after that we discuss how they are implemented/used/enforced. -Testable --------- -Unit tests heavily depend on the following two functions being defined for all types that need to be tested: - -* functions: `print`, `equals` Manifold -------- @@ -59,11 +54,21 @@ Vector Space Lie Group where compose == `+` +Testable +-------- +Unit tests heavily depend on the following two functions being defined for all types that need to be tested: + +* functions: `print`, `equals` + +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 ------ -`gtsam::traits` is our way to associate these concepts with types. We will not use Eigen-style or STL-style traits, that define many properties at once. Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming. +We will not use Eigen-style or STL-style traits, that define many properties at once. Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming. Traits allow us to play with types that are outside GTSAM control, e.g., `Eigen::VectorXd`. From 40f5188b20588122f3b55d652f876ebf1a8884d9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 19:43:52 +0100 Subject: [PATCH 008/199] OK, that should about do it... --- GTSAM-Concepts.md | 162 ++++++++++++++++++++++++++++++++-------------- 1 file changed, 113 insertions(+), 49 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 4540a0fb6..0693a758d 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -16,33 +16,56 @@ 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 the vector space. +[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 the vector space. -In GTSAM we assume that a manifold type can yield such a chart at any point, and we require that a functor `defaultChart` is available that, when called for any point on the manifold, returns a Chart type. +In GTSAM we assume that a manifold type can yield such a chart at any point, and we require that a functor `defaultChart` is available that, when called for any point on the manifold, returns a Chart type. Hence, the functor itself can be seen as an *Atlas*. -* values: `dimension` -* types: `Vector`, type that lives in tangent space - * - * `DefaultChart` is the *type* of the chart returned by the functor `defaultChart` -* functor `defaultChart`, returns a `DefaultChart` -* invariants: `defaultChart::result_type == DefaultChart::type` +In detail, we ask the following are defined for a MANIFOLD type: + +* values: + * `dimension`, an int 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. +* functors: + * `defaultChart`, returns the default chart at a point p +* types: + * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. Anything else? Chart ----- +A given chart is implemented using a small class that defines the chart itself (from manifold to tangent space) and its inverse. -* types: `Manifold`, a pointer back to the type -* values: `retract`, `local` +* types: + * `Manifold`, a pointer back to the type +* valid expressions: + * `Chart chart(p)` constructor + * `v = chart.local(q)`, the chart, from manifold to tangent space, think of it as *p (-) q* + * `p = chart.retract(v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v* -Are these values? They are just methods. Anything else? +For many differential manifolds, an obvious mapping is the `exponential map`, which associates staright 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: + + * Sometimes, most notably for *SO(3)* and *SE(3)*, the exponential map is unnecessarily expensive for use in optimiazation. Hence, the `defaultChart` functor returns a chart that is much cheaper to evaluate. + * While vector spaces (see below) are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. Hence, while a `defaultChart` functor is defined by default for every vector space, GTSAM will never call it. 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. -* values: `identity` -* values: `compose`, `inverse`, (`between`) +* values: + * `identity` +* valid expressions: + * `compose(p,q)` + * `inverse(p)` + * `between(p,q)` +* invariants: + * `compose(p,inverse(p)) == identity` + * `compose(p,between(p,q)) == q` + * `between(p,q) == compose(inverse(p),q)` + +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 --------- @@ -52,36 +75,49 @@ Implements both MANIFOLD and GROUP Vector Space ------------ -Lie Group where compose == `+` +Trivial Lie Group where + + * `identity == 0` + * `inverse(p) == -p` + * `compose(p,q) == p+q` + * `between(p,q) == q-p` + * `chart.retract(q) == p-q` + * `chart.retract(v) == p+v` + +This considerably simplifies certain operations. Testable -------- Unit tests heavily depend on the following two functions being defined for all types that need to be tested: -* functions: `print`, `equals` +* valid expressions: + * `print(p,s)` where s is an optional string + * `equals(p,q,tol)` where tol is an optional 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). +GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts. -traits +`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 ------ -We will not use Eigen-style or STL-style traits, that define many properties at once. Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming. +We will not use Eigen-style or STL-style traits, that define *many* properties at once. Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming, which return a single type or value for every trait. -Traits allow us to play with types that are outside GTSAM control, e.g., `Eigen::VectorXd`. +Traits allow us to play with types that are outside GTSAM control, e.g., `Eigen::VectorXd`. However, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types, as well, rather than having to use traits internally. -The naming conventions are as follows: +The conventions for `gtsam::traits` are as follows: -* Types: `gtsam::traits::SomeAssociatedType::type`, i.e., they are MixedCase and define a `type`, for example: +* Types: `gtsam::traits::SomeAssociatedType::type`, i.e., they are MixedCase and define a *single* `type`, for example: template<> gtsam::traits::TangentVector { typedef Vector2 type; } -* Values: `gtsam::traits::someValue::value`, i.e., they are mixedCase starting with a lowercase letter and define a `value`, but also a `value_type`. For example: +* Values: `gtsam::traits::someValue::value`, i.e., they are mixedCase starting with a lowercase letter and define a `value`, *and* a `value_type`. For example: template<> gtsam::traits::dimension { @@ -89,7 +125,7 @@ The naming conventions are as follows: typedef const int value_type; // const ? } -* Functors: `gtsam::traits::someFunctor::type`, i.e., they are mixedCase starting with a lowercase letter and define a functor `type`. The funcor itself should define a `result_type`. Example +* Functors: `gtsam::traits::someFunctor::type`, i.e., they are mixedCase starting with a lowercase letter and define a functor (i.e., no *type*). The functor itself should define a `result_type`. Example struct Point2::retract { typedef Point2 result_type; @@ -101,53 +137,62 @@ The naming conventions are as follows: } template<> - gtsam::traits::retract { - typedef Point2::retract type; - } - - The above is still up in the air. Do we need the type indirection? Could we just inherit the trait like so - - template<> - gtsam::traits::retract : Point2::retract { - } + gtsam::traits::retract : Point2::retract {} - In which case we could just say `gtsam::traits::retract(p)(v)`. + By *inherting* the trait from the functor, we can just use the [currying](http://en.wikipedia.org/wiki/Currying) style `gtsam::traits::retract(p)(v)`. Note that, although technically a functor is a type, in spirit it is a free function and hence starts with a lowercase letter. -tags +Tags ---- -Concepts are associated with a tag. +Algebraic structure concepts are associated with the following tags -* `gtsam::tags::manifold_tag` -* `gtsam::tags::group_tag` -* `gtsam::tags::lie_group_tag` -* `gtsam::tags::vector_space_tag` +* `gtsam::traits::manifold_tag` +* `gtsam::traits::group_tag` +* `gtsam::traits::lie_group_tag` +* `gtsam::traits::vector_space_tag` -Can be queried `gtsam::traits::structure_tag` +which should be queryable by `gtsam::traits::structure` +The group composition operation can be of two flavors: +* `gtsam::traits::additive_group_tag` +* `gtsam::traits::multiplicative_group_tag` + +which should be queryable by `gtsam::traits::group_flavor` Examples -------- -An example of implementing a Manifold is here: +An example of implementing a Manifold type is here: // GTSAM type class Rot2 { - ... + typedef Vector2 TangentVector; class Chart { - ... + Chart(const Rot2& R); + TangentVector local(const Rot2& R) const; + Rot2 retract(const TangentVector& v) const; } + Rot2 operator*(const Rot2&) const; + Rot2 transpose() const; } - namespace gtsam { - namespace traits { + namespace gtsam { namespace traits { template<> - struct DefaultChart { - typedef Rot2::Chart type; + struct dimension { + static const int value = 2; + typedef int value_type; } + template<> + struct TangentVector { + typedef Rot2::TangentVector type; + } + + template<> + struct defaultChart : Rot2::Chart {} + template<> struct Manifold { typedef Rot2 type; @@ -157,7 +202,26 @@ An example of implementing a Manifold is here: struct Vector { typedef Vector2 type; } - } - + + }} + +But Rot2 is in fact also a Lie Group, after we define + + Rot2 inverse(const Rot2& p) { return p.transpose();} + Rot2 operator*(const Rot2& p, const Rot2& q) { return p*q;} + Rot2 compose(const Rot2& p, const Rot2& q) { return p*q;} + Rot2 between(const Rot2& p, const Rot2& q) { return p*q;} + +The only traits that needs to be implemented are the tags: + + namespace gtsam { namespace traits { + + template<> + struct structure : lie_group_tag {} + + template<> + struct group_flavor : multiplicative_group_tag {} + + }} From 80faf61627ad0200aff3ac32bba974e78d6c2425 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 20:15:19 +0100 Subject: [PATCH 009/199] Fixed some top-level files --- README.md | 10 ++++++---- THANKS | 21 ++++++++++++++++++++- USAGE => USAGE.md | 41 +++++++---------------------------------- 3 files changed, 33 insertions(+), 39 deletions(-) rename USAGE => USAGE.md (57%) diff --git a/README.md b/README.md index 623b1ff32..679af5a2f 100644 --- a/README.md +++ b/README.md @@ -40,10 +40,12 @@ Optional prerequisites - used automatically if findable by CMake: Additional Information ---------------------- -Read about important [`GTSAM-Concepts`] here. +Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here. -See the [`INSTALL`] file for more detailed installation instructions. +See the [`INSTALL`](INSTALL) file for more detailed installation instructions. -GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files. +GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files. -Please see the [`examples/`](examples) directory and the [`USAGE`] file for examples on how to use GTSAM. \ No newline at end of file +Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM. + +GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS). \ No newline at end of file diff --git a/THANKS b/THANKS index 0d5d9db30..d7e1d1082 100644 --- a/THANKS +++ b/THANKS @@ -1,20 +1,39 @@ GTSAM was made possible by the efforts of many collaborators at Georgia Tech +Sungtae An Doru Balcan Chris Beall +Luca Carlone Alex Cunningham +Jing Dong Alireza Fathi Eohan George +Alex Hagiopol Viorela Ila Yong-Dian Jian Michael Kaess +Zhaoyang Lv +Andrew Melim Kai Ni Carlos Nieto -Duy-Nguyen +Duy-Nguyen Ta Manohar Paluri Christian Potthast Richard Roberts Grant Schindler +Natesh Srinivasan +Thomas Schneider +Alex Trevor + +at ETH, Zurich + +Paul Furgale +Mike Bosse +Hannes Sommer + +at Uni Zurich: + +Christian Forster Many thanks for your hard work!!!! Frank Dellaert diff --git a/USAGE b/USAGE.md similarity index 57% rename from USAGE rename to USAGE.md index a41b71045..0493db680 100644 --- a/USAGE +++ b/USAGE.md @@ -1,6 +1,5 @@ USAGE - Georgia Tech Smoothing and Mapping library ---------------------------------------------------- - +=================================== What is this file? This file explains how to make use of the library for common SLAM tasks, @@ -34,18 +33,12 @@ The GTSAM library has three primary components necessary for the construction of factor graph representation and optimization which users will need to adapt to their particular problem. -FactorGraph: - A factor graph contains a set of variables to solve for (i.e., robot poses, - landmark poses, etc.) and a set of constraints between these variables, which - make up factors. -Values: - Values is a single object containing labeled values for all of the - variables. Currently, all variables are labeled with strings, but the type - or organization of the variables can change -Factors: - A nonlinear factor expresses a constraint between variables, which in the - SLAM example, is a measurement such as a visual reading on a landmark or - odometry. +* FactorGraph: + A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors. +* Values: + Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change +* Factors: + A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry. The library is organized according to the following directory structure: @@ -59,23 +52,3 @@ The library is organized according to the following directory structure: -VSLAM Example ---------------------------------------------------- -The visual slam example shows a full implementation of a slam system. The example contains -derived versions of NonlinearFactor, NonlinearFactorGraph, in classes visualSLAM::ProjectionFactor, -visualSLAM::Graph, respectively. The values for the system are stored in the generic -Values structure. For definitions and interface, see gtsam/slam/visualSLAM.h. - -The clearest example of the use of the graph to find a solution is in -testVSLAM. The basic process for using graphs is as follows (and can be seen in -the test): - - Create a NonlinearFactorGraph object (visualSLAM::Graph) - - Add factors to the graph (note the use of Boost.shared_ptr here) (visualSLAM::ProjectionFactor) - - Create an initial configuration (Values) - - Create an elimination ordering of variables (this must include all variables) - - Create and initialize a NonlinearOptimizer object (Note that this is a generic - algorithm that does not need to be derived for a particular problem) - - Call optimization functions with the optimizer to optimize the graph - - Extract an updated values from the optimizer - - \ No newline at end of file From 93593f19904ff9f2e4f64b4212cb1acb27a8dff0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 21:05:57 +0100 Subject: [PATCH 010/199] Point2 example and MACRO proposal --- GTSAM-Concepts.md | 70 +++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 62 insertions(+), 8 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 0693a758d..469659661 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -160,8 +160,8 @@ The group composition operation can be of two flavors: which should be queryable by `gtsam::traits::group_flavor` -Examples --------- +Manifold Example +---------------- An example of implementing a Manifold type is here: @@ -198,11 +198,6 @@ An example of implementing a Manifold type is here: typedef Rot2 type; } - template<> - struct Vector { - typedef Vector2 type; - } - }} But Rot2 is in fact also a Lie Group, after we define @@ -210,7 +205,7 @@ But Rot2 is in fact also a Lie Group, after we define Rot2 inverse(const Rot2& p) { return p.transpose();} Rot2 operator*(const Rot2& p, const Rot2& q) { return p*q;} Rot2 compose(const Rot2& p, const Rot2& q) { return p*q;} - Rot2 between(const Rot2& p, const Rot2& q) { return p*q;} + Rot2 between(const Rot2& p, const Rot2& q) { return inverse(p)*q;} The only traits that needs to be implemented are the tags: @@ -224,4 +219,63 @@ The only traits that needs to be implemented are the tags: }} +Vector Space Example +-------------------- + +Providing the Vector space concept is easier: + + // GTSAM type + class Point2 { + static const int dimension = 2; + Point2 operator+(const Point2&) const; + Point2 operator-(const Point2&) const; + Point2 operator-() const; + // Still needs to be defined, unless Point2 *is* Vector2 + class Chart { + Chart(const Rot2& R); + Vector2 local(const Rot2& R) const; + Rot2 retract(const Vector2& v) const; + } + } + + The following macro, called inside the gtsam namespace, + + DEFINE_VECTOR_SPACE_TRAITS(Point2) + + should automatically define + + namespace traits { + + template<> + struct dimension { + static const int value = Point2::dimension; + typedef int value_type; + } + + template<> + struct TangentVector { + typedef Matrix::Eigen type; + } + + template<> + struct defaultChart : Point2::Chart {} + + template<> + struct Manifold { + typedef Point2 type; + } + + template<> + struct structure : vector_space_tag {} + + template<> + struct group_flavor : additive_group_tag {} + } + +and + + Point2 inverse(const Point2& p) { return p.transpose();} + Point2 operator+(const Point2& p, const Point2& q) { return p+q;} + Point2 compose(const Point2& p, const Point2& q) { return p+q;} + Point2 between(const Point2& p, const Point2& q) { return q-p;} From b5f64fe1b46ee907960896306c561b0d692ad8a4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 21:12:03 +0100 Subject: [PATCH 011/199] Fixed types - Thanks @mikebosse --- GTSAM-Concepts.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 469659661..3e390fda6 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -44,7 +44,7 @@ A given chart is implemented using a small class that defines the chart itself ( For many differential manifolds, an obvious mapping is the `exponential map`, which associates staright 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: - * Sometimes, most notably for *SO(3)* and *SE(3)*, the exponential map is unnecessarily expensive for use in optimiazation. Hence, the `defaultChart` functor returns a chart that is much cheaper to evaluate. + * Sometimes, most notably for *SO(3)* and *SE(3)*, the exponential map is unnecessarily expensive for use in optimization. Hence, the `defaultChart` functor returns a chart that is much cheaper to evaluate. * While vector spaces (see below) are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. Hence, while a `defaultChart` functor is defined by default for every vector space, GTSAM will never call it. @@ -132,7 +132,7 @@ The conventions for `gtsam::traits` are as follows: Point2 p_; retract(const Point2& p) : p_(p) {} Point2 operator()(const Vector2& v) { - return Point2(p.x()+v[0], p.y()+v[1]); + return Point2(p_.x()+v[0], p_.y()+v[1]); } } @@ -274,7 +274,7 @@ Providing the Vector space concept is easier: and - Point2 inverse(const Point2& p) { return p.transpose();} + Point2 inverse(const Point2& p) { return -p;} Point2 operator+(const Point2& p, const Point2& q) { return p+q;} Point2 compose(const Point2& p, const Point2& q) { return p+q;} Point2 between(const Point2& p, const Point2& q) { return q-p;} From dae420e38f39a847570e9f64e0b5fc6ec04b5675 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 21:48:17 +0100 Subject: [PATCH 012/199] manhattan example --- GTSAM-Concepts.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 3e390fda6..e01a57561 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -42,7 +42,7 @@ A given chart is implemented using a small class that defines the chart itself ( * `v = chart.local(q)`, the chart, from manifold to tangent space, think of it as *p (-) q* * `p = chart.retract(v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v* -For many differential manifolds, an obvious mapping is the `exponential map`, which associates staright 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: +For many differential manifolds, an obvious mapping is the `exponential map`, which 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: * Sometimes, most notably for *SO(3)* and *SE(3)*, the exponential map is unnecessarily expensive for use in optimization. Hence, the `defaultChart` functor returns a chart that is much cheaper to evaluate. * While vector spaces (see below) are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. Hence, while a `defaultChart` functor is defined by default for every vector space, GTSAM will never call it. @@ -125,21 +125,21 @@ The conventions for `gtsam::traits` are as follows: typedef const int value_type; // const ? } -* Functors: `gtsam::traits::someFunctor::type`, i.e., they are mixedCase starting with a lowercase letter and define a functor (i.e., no *type*). The functor itself should define a `result_type`. Example +* Functors: `gtsam::traits::someFunctor::type`, i.e., they are mixedCase starting with a lowercase letter and define a functor (i.e., no *type*). The functor itself should define a `result_type`. A contrived example - struct Point2::retract { - typedef Point2 result_type; + struct Point2::manhattan { + typedef double result_type; Point2 p_; - retract(const Point2& p) : p_(p) {} - Point2 operator()(const Vector2& v) { - return Point2(p_.x()+v[0], p_.y()+v[1]); + manhattan(const Point2& p) : p_(p) {} + Point2 operator()(const Point2& q) { + return abs(p_.x()-q.x()) + abs(p_.y()-q.x()); } } template<> - gtsam::traits::retract : Point2::retract {} + gtsam::traits::manhattan : Point2::manhattan {} - By *inherting* the trait from the functor, we can just use the [currying](http://en.wikipedia.org/wiki/Currying) style `gtsam::traits::retract(p)(v)`. Note that, although technically a functor is a type, in spirit it is a free function and hence starts with a lowercase letter. + By *inherting* the trait from the functor, we can just use the [currying](http://en.wikipedia.org/wiki/Currying) style `gtsam::traits::manhattan℗(q)`. Note that, although technically a functor is a type, in spirit it is a free function and hence starts with a lowercase letter. Tags ---- From f8cc5947901cf00749038d85511f50ed473f6f38 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 22:09:35 +0100 Subject: [PATCH 013/199] getDimension (moved from chart) and concept checks --- GTSAM-Concepts.md | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index e01a57561..7858e8390 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -28,6 +28,8 @@ In detail, we ask the following are defined for a MANIFOLD type: * `defaultChart`, returns the default chart at a point p * types: * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. +* valid expressions: + * `size_t dim = getDimension(p);` free function should be defined in case the dimension is not known at compile time. Anything else? @@ -108,6 +110,8 @@ We will not use Eigen-style or STL-style traits, that define *many* properties a Traits allow us to play with types that are outside GTSAM control, e.g., `Eigen::VectorXd`. However, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types, as well, rather than having to use traits internally. +Finally, note that not everything that makes a concept is defined by traits. For example, although a CHART type is supposed to have a `retract` function, there is no trait for this: rather, the + The conventions for `gtsam::traits` are as follows: * Types: `gtsam::traits::SomeAssociatedType::type`, i.e., they are MixedCase and define a *single* `type`, for example: @@ -160,6 +164,7 @@ The group composition operation can be of two flavors: which should be queryable by `gtsam::traits::group_flavor` + Manifold Example ---------------- @@ -279,3 +284,35 @@ and Point2 compose(const Point2& p, const Point2& q) { return p+q;} Point2 between(const Point2& p, const Point2& q) { return q-p;} +Concept Checks +-------------- + +Boost provides a nice way to check whether a given type satisfies a concept. For example, the following + + BOOST_CONCEPT_ASSERT(ChartConcept) + +Using the following from Mike Bosse's prototype: + +``` +template +struct ChartConcept { + + typedef gtsam::traits::Manifold::type type; + typedef gtsam::traits::TangentVector::type vector; + + BOOST_CONCEPT_USAGE(ChartConcept) { + + // Returns Retraction update of val_ + type retract_ret = C::retract(val_, vec_); + + // Returns local coordinates of another object + vec_ = C::local(val_, retract_ret); + } + +private: + type val_; + vector vec_; + int dim_; +}; + +``` From 4458325c171e36a976cfbd541c4f9a99283b4c3a Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 22:14:18 +0100 Subject: [PATCH 014/199] c== tag ? --- GTSAM-Concepts.md | 1 + 1 file changed, 1 insertion(+) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 7858e8390..3d425a4df 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -294,6 +294,7 @@ Boost provides a nice way to check whether a given type satisfies a concept. For Using the following from Mike Bosse's prototype: ``` +#!c++ template struct ChartConcept { From ae64104602720b9df63569a9dbbabcdc3216cd59 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 3 Dec 2014 22:24:49 +0100 Subject: [PATCH 015/199] More c++ tags to make code readable when 'Viewing' file --- GTSAM-Concepts.md | 40 +++++++++++++++++++++++++++++++++------- 1 file changed, 33 insertions(+), 7 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 3d425a4df..15a8003d1 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -170,6 +170,8 @@ Manifold Example An example of implementing a Manifold type is here: +``` +#!c++ // GTSAM type class Rot2 { typedef Vector2 TangentVector; @@ -204,17 +206,27 @@ An example of implementing a Manifold type is here: } }} +``` But Rot2 is in fact also a Lie Group, after we define +``` +#!c++ +namespace manifold { Rot2 inverse(const Rot2& p) { return p.transpose();} Rot2 operator*(const Rot2& p, const Rot2& q) { return p*q;} Rot2 compose(const Rot2& p, const Rot2& q) { return p*q;} Rot2 between(const Rot2& p, const Rot2& q) { return inverse(p)*q;} +} +``` The only traits that needs to be implemented are the tags: - namespace gtsam { namespace traits { + +``` +#!c++ +namespace gtsam { + namespace traits { template<> struct structure : lie_group_tag {} @@ -222,13 +234,18 @@ The only traits that needs to be implemented are the tags: template<> struct group_flavor : multiplicative_group_tag {} - }} + } +} +``` Vector Space Example -------------------- Providing the Vector space concept is easier: + +``` +#!c++ // GTSAM type class Point2 { static const int dimension = 2; @@ -242,14 +259,17 @@ Providing the Vector space concept is easier: Rot2 retract(const Vector2& v) const; } } - +``` + The following macro, called inside the gtsam namespace, DEFINE_VECTOR_SPACE_TRAITS(Point2) should automatically define - - namespace traits { + +``` +#!c++ +namespace traits { template<> struct dimension { @@ -275,14 +295,19 @@ Providing the Vector space concept is easier: template<> struct group_flavor : additive_group_tag {} - } - +} +``` and +``` +#!c++ +namespace manifold { Point2 inverse(const Point2& p) { return -p;} Point2 operator+(const Point2& p, const Point2& q) { return p+q;} Point2 compose(const Point2& p, const Point2& q) { return p+q;} Point2 between(const Point2& p, const Point2& q) { return q-p;} +} +``` Concept Checks -------------- @@ -293,6 +318,7 @@ Boost provides a nice way to check whether a given type satisfies a concept. For Using the following from Mike Bosse's prototype: + ``` #!c++ template From 8b20175b490f529e8dc484345968dbec7cd4c1b2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 4 Dec 2014 01:12:18 +0100 Subject: [PATCH 016/199] Mostly changes in THANKS --- GTSAM-Concepts.md | 4 +-- THANKS | 62 +++++++++++++++++++++++++---------------------- 2 files changed, 35 insertions(+), 31 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 15a8003d1..41bdafee9 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -314,7 +314,7 @@ Concept Checks Boost provides a nice way to check whether a given type satisfies a concept. For example, the following - BOOST_CONCEPT_ASSERT(ChartConcept) + BOOST_CONCEPT_ASSERT(ChartConcept >) Using the following from Mike Bosse's prototype: @@ -328,7 +328,7 @@ struct ChartConcept { typedef gtsam::traits::TangentVector::type vector; BOOST_CONCEPT_USAGE(ChartConcept) { - + // Returns Retraction update of val_ type retract_ret = C::retract(val_, vec_); diff --git a/THANKS b/THANKS index d7e1d1082..9c06a5d28 100644 --- a/THANKS +++ b/THANKS @@ -1,39 +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: -Sungtae An -Doru Balcan -Chris Beall -Luca Carlone -Alex Cunningham -Jing Dong -Alireza Fathi -Eohan George -Alex Hagiopol -Viorela Ila -Yong-Dian Jian -Michael Kaess -Zhaoyang Lv -Andrew Melim -Kai Ni -Carlos Nieto -Duy-Nguyen Ta -Manohar Paluri -Christian Potthast -Richard Roberts -Grant Schindler -Natesh Srinivasan -Thomas Schneider -Alex Trevor +* 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 +* Paul Furgale +* Mike Bosse +* Hannes Sommer +* Thomas Schneider at Uni Zurich: -Christian Forster +* Christian Forster Many thanks for your hard work!!!! + Frank Dellaert From c225ee223c72abda2d74962d642ae8b6652e3fde Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Thu, 4 Dec 2014 11:27:25 +0100 Subject: [PATCH 017/199] figureing out concept checks (not done yet!!) --- gtsam/base/concepts.h | 71 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 gtsam/base/concepts.h diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h new file mode 100644 index 000000000..05d7e378b --- /dev/null +++ b/gtsam/base/concepts.h @@ -0,0 +1,71 @@ +/* + * concepts.h + * + * Created on: Dec 4, 2014 + * Author: mike bosse + */ + +#ifndef CONCEPTS_H_ +#define CONCEPTS_H_ + +#include "manifold.h" +#include "chart.h" + +namespace gtsam { + +namespace traits { + +template +struct TangentVector { + typedef Eigen::VectorXd type; +}; + +template +struct Manifold { + typedef Chart::value_type type; +}; + +} // namespace traits + +template +class ManifoldConcept { + public: + typedef T Manifold; + typedef traits::TangentVector::type TangentVector; + typedef traits::DefaultChart DefaultChart; + static const size_t dim = traits::dimension::value; + + BOOST_CONCEPT_USAGE(ManifoldConcept) { + + // assignable + T t2 = t; + + TangentVector v; + BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); + + } + private: + Manifold p; +}; + +template +class ChartConcept { + public: + typedef C Chart; + typedef typename traits::Manifold::type Manifold; + typedef typename traits::TangentVector::type TangentVector; + + BOOST_CONCEPT_USAGE(ChartConcept) { + v = Chart::local(p,q); // returns local coordinates of q w.r.t. origin p + q = Chart::retract(p,v); // returns retracted update of p with v + } + + private: + Manifold p,q; + TangentVector v; + +}; + +} // namespace gtsam + +#endif /* CONCEPTS_H_ */ From b6576d7e276599af0517ebad42233047d753992e Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Thu, 4 Dec 2014 13:10:50 +0100 Subject: [PATCH 018/199] added group concept check --- GTSAM-Concepts.md | 2 +- gtsam/base/concepts.h | 23 +++++++++++++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 41bdafee9..774409e8d 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -162,7 +162,7 @@ The group composition operation can be of two flavors: * `gtsam::traits::additive_group_tag` * `gtsam::traits::multiplicative_group_tag` -which should be queryable by `gtsam::traits::group_flavor` +which should be queryable by `gtsam::traits::group_flavor::tag` Manifold Example diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 05d7e378b..90f897b90 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -66,6 +66,29 @@ class ChartConcept { }; +template +class GroupConcept { + public: + typedef G Group; + static const Group identity = traits::identity::value; + + BOOST_CONCEPT_USAGE(GroupConcept) { + Group ip = inverse(p); + Group pq = compose(p, q); + Group d = between(p, q); + bool test = equal(p, q); + } + + bool check_invariants(const Group& a, const Group& b) { + return (equal(compose(a, inverse(a)), identity)) + && (equal(between(a, b), compose(inverse(a), b))) + && (equal(compose(a, between(a, b)), b)); + } + + private: + Group p,q; +}; + } // namespace gtsam #endif /* CONCEPTS_H_ */ From 978c17ceb6f97ea06250567bb5914bef7ec9a400 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Thu, 4 Dec 2014 13:11:45 +0100 Subject: [PATCH 019/199] group concept operator usage with tags --- gtsam/base/concepts.h | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 90f897b90..e4fa6eda9 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -25,6 +25,9 @@ struct Manifold { typedef Chart::value_type type; }; +struct additive_group_tag {}; +struct multiplicative_group_tag {}; + } // namespace traits template @@ -77,16 +80,27 @@ class GroupConcept { Group pq = compose(p, q); Group d = between(p, q); bool test = equal(p, q); + operator_usage(p, q, traits::group_flavor::tag); } bool check_invariants(const Group& a, const Group& b) { return (equal(compose(a, inverse(a)), identity)) && (equal(between(a, b), compose(inverse(a), b))) - && (equal(compose(a, between(a, b)), b)); + && (equal(compose(a, between(a, b)), b)) + && operator_usage(a, b, traits::group_flavor::tag) } private: Group p,q; + + bool operator_usage(const Group& a, const Group& b, traits::multiplicative_group_tag) { + return equals(compose(a, b), a*b); + + } + bool operator_usage(const Group& a, const Group& b, traits::additive_group_tag) { + return equals(compose(a, b), a+b); + } + }; } // namespace gtsam From eda6bf5ffe43c7f0f3d743c59650e75603d7fc8c Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Thu, 4 Dec 2014 13:18:34 +0100 Subject: [PATCH 020/199] minor --- gtsam/base/concepts.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index e4fa6eda9..683b29d70 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -80,24 +80,24 @@ class GroupConcept { Group pq = compose(p, q); Group d = between(p, q); bool test = equal(p, q); - operator_usage(p, q, traits::group_flavor::tag); + bool test2 = operator_usage(p, q, traits::group_flavor); } bool check_invariants(const Group& a, const Group& b) { return (equal(compose(a, inverse(a)), identity)) && (equal(between(a, b), compose(inverse(a), b))) && (equal(compose(a, between(a, b)), b)) - && operator_usage(a, b, traits::group_flavor::tag) + && operator_usage(a, b, traits::group_flavor) } private: Group p,q; - bool operator_usage(const Group& a, const Group& b, traits::multiplicative_group_tag) { + bool operator_usage(const Group& a, const Group& b, const traits::multiplicative_group_tag&) { return equals(compose(a, b), a*b); } - bool operator_usage(const Group& a, const Group& b, traits::additive_group_tag) { + bool operator_usage(const Group& a, const Group& b, const traits::additive_group_tag&) { return equals(compose(a, b), a+b); } From 56787e1a882cbb9f0811485e8c750b5fef680323 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Thu, 4 Dec 2014 15:26:01 +0100 Subject: [PATCH 021/199] addressed review comments --- gtsam/base/concepts.h | 29 +++++++++++++++++------------ 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 683b29d70..e4f5924de 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -17,14 +17,23 @@ namespace traits { template struct TangentVector { - typedef Eigen::VectorXd type; + //typedef XXX type; }; +// used to identify the manifold associated with a chart template struct Manifold { - typedef Chart::value_type type; + //typedef XXX type; }; +template +struct DefaultChart { + //typedef XXX type; +}; + +template +struct group_flavor {}; + struct additive_group_tag {}; struct multiplicative_group_tag {}; @@ -35,20 +44,16 @@ class ManifoldConcept { public: typedef T Manifold; typedef traits::TangentVector::type TangentVector; - typedef traits::DefaultChart DefaultChart; + typedef traits::DefaultChart::type DefaultChart; static const size_t dim = traits::dimension::value; BOOST_CONCEPT_USAGE(ManifoldConcept) { - - // assignable - T t2 = t; - - TangentVector v; BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - + // no direct usage for manifold since most usage is through a chart } private: Manifold p; + TangentVector v; }; template @@ -87,18 +92,18 @@ class GroupConcept { return (equal(compose(a, inverse(a)), identity)) && (equal(between(a, b), compose(inverse(a), b))) && (equal(compose(a, between(a, b)), b)) - && operator_usage(a, b, traits::group_flavor) + && operator_usage(a, b, traits::group_flavor); } private: Group p,q; bool operator_usage(const Group& a, const Group& b, const traits::multiplicative_group_tag&) { - return equals(compose(a, b), a*b); + return equal(compose(a, b), a*b); } bool operator_usage(const Group& a, const Group& b, const traits::additive_group_tag&) { - return equals(compose(a, b), a+b); + return equal(compose(a, b), a+b); } }; From 2fa9d0f557ac2bb3000e87754d8124e32ab2b9c7 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Thu, 4 Dec 2014 15:57:41 +0100 Subject: [PATCH 022/199] LieGroup concept and flushing out more tags --- gtsam/base/concepts.h | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index e4f5924de..b231dfe0d 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -10,6 +10,7 @@ #include "manifold.h" #include "chart.h" +#include namespace gtsam { @@ -31,9 +32,15 @@ struct DefaultChart { //typedef XXX type; }; +template +struct structure {}; // specializations should be derived from one of the following tags +struct manifold_tag {}; +struct group_tag {}; +struct lie_group_tag : public manifold_tag, public group_tag {}; +struct vector_space_tag : public lie_group_tag {}; + template struct group_flavor {}; - struct additive_group_tag {}; struct multiplicative_group_tag {}; @@ -43,11 +50,13 @@ template class ManifoldConcept { public: typedef T Manifold; + typedef traits::manifold_tag manifold_tag; typedef traits::TangentVector::type TangentVector; typedef traits::DefaultChart::type DefaultChart; static const size_t dim = traits::dimension::value; BOOST_CONCEPT_USAGE(ManifoldConcept) { + BOOST_STATIC_ASSERT(boost::is_base_of >); BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); // no direct usage for manifold since most usage is through a chart } @@ -81,6 +90,7 @@ class GroupConcept { static const Group identity = traits::identity::value; BOOST_CONCEPT_USAGE(GroupConcept) { + BOOST_STATIC_ASSERT(boost::is_base_of >); Group ip = inverse(p); Group pq = compose(p, q); Group d = between(p, q); @@ -108,6 +118,16 @@ class GroupConcept { }; +template +class LieGroupConcept : public GroupConcept, public ManifoldConcept { + + BOOST_CONCEPT_USAGE(LieGroupConcept) { + BOOST_STATIC_ASSERT(boost::is_base_of >); + } +}; + + + } // namespace gtsam #endif /* CONCEPTS_H_ */ From b5e3c8816dde8fea530906157ce3dcef05e5b026 Mon Sep 17 00:00:00 2001 From: Mike Bosse Date: Thu, 4 Dec 2014 22:15:20 +0100 Subject: [PATCH 023/199] added vector space concept check, but not sure it will work... --- gtsam/base/concepts.h | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index b231dfe0d..cc86d98ac 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -50,7 +50,6 @@ template class ManifoldConcept { public: typedef T Manifold; - typedef traits::manifold_tag manifold_tag; typedef traits::TangentVector::type TangentVector; typedef traits::DefaultChart::type DefaultChart; static const size_t dim = traits::dimension::value; @@ -126,7 +125,29 @@ class LieGroupConcept : public GroupConcept, public ManifoldConcept { } }; +template +class VectorSpaceConcept : public LieGroupConcept { + typedef traits::DefaultChart::type Chart; + typedef GroupConcept::identity identity; + BOOST_CONCEPT_USAGE(VectorSpaceConcept) { + BOOST_STATIC_ASSERT(boost::is_base_of >); + r = p+q; + r = -p; + r = p-q; + } + + bool check_invariants(const V& a, const V& b) { + return equal(compose(a, b), a+b) + && equal(inverse(a), -a) + && equal(between(a, b), b-a) + && equal(Chart::retract(a, b), a+b) + && equal(Chart::local(a, b), b-a); + } + + private: + V p,q,r; +}; } // namespace gtsam From 493b38ef28753bcb5269d538c48a4ca825d71a3e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Dec 2014 22:01:14 +0000 Subject: [PATCH 024/199] Made charts have static methods, defined traits convention for tags. --- GTSAM-Concepts.md | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 774409e8d..00fb3a86e 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -40,9 +40,8 @@ A given chart is implemented using a small class that defines the chart itself ( * types: * `Manifold`, a pointer back to the type * valid expressions: - * `Chart chart(p)` constructor - * `v = chart.local(q)`, the chart, from manifold to tangent space, think of it as *p (-) q* - * `p = chart.retract(v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v* + * `v = Chart::local(p,q)`, the chart, from manifold to tangent space, think of it as *p (-) q* + * `p = Chart::retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v* For many differential manifolds, an obvious mapping is the `exponential map`, which 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: @@ -106,7 +105,7 @@ GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the T Traits ------ -We will not use Eigen-style or STL-style traits, that define *many* properties at once. Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming, which return a single type or value for every trait. +We will not use Eigen-style or STL-style traits, that define *many* properties at once. Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming, which return a single type or value for every trait. Some rationale/history can be found [here](http://www.boost.org/doc/libs/1_55_0/libs/type_traits/doc/html/boost_typetraits/background.html). Traits allow us to play with types that are outside GTSAM control, e.g., `Eigen::VectorXd`. However, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types, as well, rather than having to use traits internally. @@ -145,6 +144,15 @@ The conventions for `gtsam::traits` are as follows: By *inherting* the trait from the functor, we can just use the [currying](http://en.wikipedia.org/wiki/Currying) style `gtsam::traits::manhattan℗(q)`. Note that, although technically a functor is a type, in spirit it is a free function and hence starts with a lowercase letter. +* Tags: `gtsam::traits::some_category::type`, i.e., they are lower_case and define a *single* `type`, for example: + + template<> + gtsam::traits::structure_category { + typedef vector_space_tag type; + } + + See below for the tags defined within GTSAM. + Tags ---- @@ -155,14 +163,14 @@ Algebraic structure concepts are associated with the following tags * `gtsam::traits::lie_group_tag` * `gtsam::traits::vector_space_tag` -which should be queryable by `gtsam::traits::structure` +which should be queryable by `gtsam::traits::structure_category::type` The group composition operation can be of two flavors: * `gtsam::traits::additive_group_tag` * `gtsam::traits::multiplicative_group_tag` -which should be queryable by `gtsam::traits::group_flavor::tag` +which should be queryable by `gtsam::traits::group_flavor::type` Manifold Example @@ -176,9 +184,8 @@ An example of implementing a Manifold type is here: class Rot2 { typedef Vector2 TangentVector; class Chart { - Chart(const Rot2& R); - TangentVector local(const Rot2& R) const; - Rot2 retract(const TangentVector& v) const; + static TangentVector local(const Rot2& R1, const Rot2& R2); + static Rot2 retract(const Rot2& R, const TangentVector& v); } Rot2 operator*(const Rot2&) const; Rot2 transpose() const; @@ -254,9 +261,8 @@ Providing the Vector space concept is easier: Point2 operator-() const; // Still needs to be defined, unless Point2 *is* Vector2 class Chart { - Chart(const Rot2& R); - Vector2 local(const Rot2& R) const; - Rot2 retract(const Vector2& v) const; + static Vector2 local(const Point2& p, const Point2& q) const; + static Rot2 retract(const Point2& p, const Point2& v) const; } } ``` From 078a178701a4a56305cfeffbdca556b2acb4202e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Dec 2014 22:27:29 +0000 Subject: [PATCH 025/199] tag dispatching and template meta-programming --- GTSAM-Concepts.md | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 00fb3a86e..b3f366303 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -172,6 +172,44 @@ The group composition operation can be of two flavors: which should be queryable by `gtsam::traits::group_flavor::type` +A tag can be used for [tag dispatching](http://www.boost.org/community/generic_programming.html#tag_dispatching), +e.g., below is a generic compose: + +``` +#!c++ + namespace detail { + template + T compose(const T& p, const T& q, additive_group_tag) { + return p + q; + } + + template + T compose(const T& p, const T& q, multiplicative_group_tag) { + return p * q; + } + } + + template + T compose(const T& p, const T& q) { + return detail::compose(p, q, traits::group_flavor::type); + } +``` + +Tags also facilitate meta-programming. Taking a leaf from [The boost Graph library](http://www.boost.org/doc/libs/1_40_0/boost/graph/graph_traits.hpp), +tags can be used to create useful meta-functions, like `is_lie_group`, below. + +``` +#!c++ + template + struct is_lie_group + : mpl::bool_< + is_convertible< + typename structure_category::type, + lie_group_tag + >::value + > + { }; +``` Manifold Example ---------------- From 5980575fe65894fca62a16524d8e77c4c5cd3145 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 4 Dec 2014 22:33:31 +0000 Subject: [PATCH 026/199] q-p --- GTSAM-Concepts.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index b3f366303..3845d7a17 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -40,7 +40,7 @@ A given chart is implemented using a small class that defines the chart itself ( * types: * `Manifold`, a pointer back to the type * valid expressions: - * `v = Chart::local(p,q)`, the chart, from manifold to tangent space, think of it as *p (-) q* + * `v = Chart::local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p* * `p = Chart::retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v* For many differential manifolds, an obvious mapping is the `exponential map`, which 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: From c975382b59e36ca8bf7639f71606536025eb6960 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 5 Dec 2014 17:03:08 +0100 Subject: [PATCH 027/199] A group acting on another space, examples for groups and lie groups ( with @pdrews ) --- GTSAM-Concepts.md | 109 ++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 101 insertions(+), 8 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 3845d7a17..8a841e94a 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -16,9 +16,9 @@ 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 the vector space. +[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 we assume that a manifold type can yield such a chart at any point, and we require that a functor `defaultChart` is available that, when called for any point on the manifold, returns a Chart type. Hence, the functor itself can be seen as an *Atlas*. +In GTSAM we assume that a manifold type can yield such a *Chart* at any point, and we require that a functor `defaultChart` is available that, when called for any point on the manifold, returns a Chart type. Hence, the functor itself can be seen as an *Atlas*. In detail, we ask the following are defined for a MANIFOLD type: @@ -54,12 +54,12 @@ 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. * values: - * `identity` + * `group::identity()` * valid expressions: - * `compose(p,q)` - * `inverse(p)` - * `between(p,q)` -* invariants: + * `group::compose(p,q)` + * `group::inverse(p)` + * `group::between(p,q)` +* invariants (using namespace group): * `compose(p,inverse(p)) == identity` * `compose(p,between(p,q)) == q` * `between(p,q) == compose(inverse(p),q)` @@ -68,10 +68,35 @@ We do *not* at this time support more than one composition operator per type. Al 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). +A group can *act* on another space. For example, a *similarity transform* in 3D can act on 3D space, like + + q = s*R*p + t + +Even finite groups can act on continuous entities. 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: + * `group::act(g,t)`, for some instance of a space T, that can be acted upon by the group + * `group::act(g,t,H)`, if the group acted upon is a continuous differentiable manifold + Lie Group --------- -Implements both MANIFOLD and GROUP +A Lie group is both a manofold *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 expressions: + +* `compose(p,q,H1,H2)` +* `inverse(p,H)` +* `between(p,q,H1,H2)` + +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 (BteweenFactor). + +when a Lie group acts on a space, we have two derivatives to care about: + + * `group::act(g,t,Hg,Ht)`, if the group acted upon is a continuous differentiable manifold Vector Space ------------ @@ -283,6 +308,74 @@ namespace gtsam { } ``` +Group Example +------------- + +As an example of a group, let's do a cyclic group, acting on Point2: + +``` +#!c++ + // GTSAM type + class Cyclic { + CyclicGroup(size_t n) {} + Cyclic operator+(const Cyclic&) const; // add modulo n + Cyclic operator-() const; // negate modulo n + + // Act on R2, rotate by i*360/n, derivative will simply be 2*2 rotation matrix + Point2 operator*(cons Point2& p, OptionalJacobian<2,2> H) const; + } + + namespace group { + // make Cyclic obey GROUP concept + Cyclic compose(const Cyclic& g, const Cyclic& h) { return g+h;} + Cyclic between(const Cyclic& g, const Cyclic& h) { return h-g;} + Cyclic inverse(const Cyclic& g) { return -p;} + + // implement acting on 2D + Vector2 act(Const Cyclic& g, cons Point2& p) { return g*p;} + } + +``` + + + +Lie Group Example +----------------- + +As an example of a Lie group, let's do a similarity transform, acting on Point3: + +``` +#!c++ + // GTSAM type + class Similarity3 { + + ... constructors and Manifold stuff... + + Similarity3 operator*(const Similarity3&) const; // compose + Similarity3 inverse() const; // matrix inverse + + // Act on R3 + Point3 operator*(cons Point3& p, + OptionalJacobian<3,7> Hg, OptionalJacobian<3,3> Hp) const { + if (Hg) *Hg << - s * R * [p], s * R, R * p; // TODO check ! + if (Hp) *Hp = s*R; + return s*R*p + t; + } + } + + namespace group { + // make Similarity3 obey GROUP concept + Similarity3 compose(const Similarity3& g, const Similarity3& h) { return g+h;} + Similarity3 between(const Similarity3& g, const Similarity3& h) { return h-g;} + Similarity3 inverse(const Similarity3& g) { return -p;} + + // implement acting on 2D + Vector2 act(Const Similarity3& g, cons Point2& p) { return g*p;} + } + +``` + + Vector Space Example -------------------- From 63effbb43364efdc72b774dbb1bd796f239afa34 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 5 Dec 2014 16:26:06 +0000 Subject: [PATCH 028/199] Small changes to latest big commit --- GTSAM-Concepts.md | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 8a841e94a..683fa9619 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -81,7 +81,7 @@ Hence, we formalize by the following extension of the concept: * valid expressions: * `group::act(g,t)`, for some instance of a space T, that can be acted upon by the group - * `group::act(g,t,H)`, if the group acted upon is a continuous differentiable manifold + * `group::act(g,t,H)`, if the space acted upon is a continuous differentiable manifold Lie Group --------- @@ -96,19 +96,21 @@ where above the `H` arguments stand for optional Jacobian arguments. That makes when a Lie group acts on a space, we have two derivatives to care about: - * `group::act(g,t,Hg,Ht)`, if the group acted upon is a continuous differentiable manifold + * `group::act(g,t,Hg,Ht)`, if the space acted upon is a continuous differentiable manifold + +For now, we won't care about Lie groups acting on non-manifolds. Vector Space ------------ -Trivial Lie Group where +Trivial Lie group where - * `identity == 0` + * `identity == 0` * `inverse(p) == -p` * `compose(p,q) == p+q` * `between(p,q) == q-p` - * `chart.retract(q) == p-q` - * `chart.retract(v) == p+v` + * `chart::retract(q) == p-q` + * `chart::retract(v) == p+v` This considerably simplifies certain operations. From dcc6c0c70116f1b233b00595cc77fbff403653b9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 5 Dec 2014 16:43:21 +0000 Subject: [PATCH 029/199] Fixed Similarity3 example, added Matrix groups --- GTSAM-Concepts.md | 33 ++++++++++++++++++++++----------- 1 file changed, 22 insertions(+), 11 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 683fa9619..4de31309a 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -86,7 +86,7 @@ Hence, we formalize by the following extension of the concept: Lie Group --------- -A Lie group is both a manofold *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 expressions: +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 expressions: * `compose(p,q,H1,H2)` * `inverse(p,H)` @@ -100,6 +100,12 @@ when a Lie group acts on a space, we have two derivatives to care about: For now, we won't care about Lie groups acting on non-manifolds. +Matrix Group +------------ + +Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of nxn invertible matrices. +In this case, a lot of the derivatives calculations needed can be standardized. + Vector Space ------------ @@ -340,7 +346,6 @@ As an example of a group, let's do a cyclic group, acting on Point2: ``` - Lie Group Example ----------------- @@ -353,26 +358,32 @@ As an example of a Lie group, let's do a similarity transform, acting on Point3: ... constructors and Manifold stuff... - Similarity3 operator*(const Similarity3&) const; // compose - Similarity3 inverse() const; // matrix inverse + Similarity3 compose(const Similarity3&, OptionalJacobian<7,7> H1, OptionalJacobian<7,7> H2) const; + Similarity3 between(const Similarity3&, OptionalJacobian<7,7> H1, OptionalJacobian<7,7> H2) const; + Similarity3 inverse(OptionalJacobian<7,7> H) const; // matrix inverse + Similarity3 operator*(const Similarity3& g) const { return compose(h);} // compose sugar + // Act on R3 - Point3 operator*(cons Point3& p, - OptionalJacobian<3,7> Hg, OptionalJacobian<3,3> Hp) const { + Point3 act(cons Point3& p, OptionalJacobian<3,7> Hg, OptionalJacobian<3,3> Hp) const { if (Hg) *Hg << - s * R * [p], s * R, R * p; // TODO check ! if (Hp) *Hp = s*R; return s*R*p + t; } + Point3 operator*(cons Point3& p); // act sugar } namespace group { // make Similarity3 obey GROUP concept - Similarity3 compose(const Similarity3& g, const Similarity3& h) { return g+h;} - Similarity3 between(const Similarity3& g, const Similarity3& h) { return h-g;} - Similarity3 inverse(const Similarity3& g) { return -p;} + Similarity3 compose(const Similarity3& g, const Similarity3& h, + OptionalJacobian<7,7> H1, OptionalJacobian<7,7> H2) { return g.operator*(p,H1,H2);} + Similarity3 between(const Similarity3& g, const Similarity3& h, + OptionalJacobian<7,7> H1, OptionalJacobian<7,7> H2) { return g.between(h,H1,H2);} + Similarity3 inverse(const Similarity3& g, OptionalJacobian<7,7> H) { return p.inverse(H);} - // implement acting on 2D - Vector2 act(Const Similarity3& g, cons Point2& p) { return g*p;} + // implement acting on 3D + Vector2 act(Const Similarity3& g, cons Point3& p, + OptionalJacobian<3,7> Hg, OptionalJacobian<3,3> Hp) { return g.act(p,Hg,Hp);} } ``` From a9bb47342408e4773422c9abbb7a23f7c704b485 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 5 Dec 2014 20:22:58 +0000 Subject: [PATCH 030/199] Made group actions a concept --- GTSAM-Concepts.md | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 4de31309a..3e324c5cd 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -68,6 +68,9 @@ We do *not* at this time support more than one composition operator per type. Al Also, a type should provide either multiplication or addition operators depending on the flavor of the operation. To distinguish between the two, we will use a tag (see below). +Group Action +------------ + A group can *act* on another space. For example, a *similarity transform* in 3D can act on 3D space, like q = s*R*p + t @@ -81,7 +84,9 @@ Hence, we formalize by the following extension of the concept: * valid expressions: * `group::act(g,t)`, for some instance of a space T, that can be acted upon by the group - * `group::act(g,t,H)`, if the space acted upon is a continuous differentiable manifold + * `group::act(g,t,H)`, if the space acted upon is a continuous differentiable manifold + +Group actions are concepts in and of themselves that can be concept checked (see below). Lie Group --------- @@ -94,7 +99,10 @@ A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should imp 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 (BteweenFactor). -when a Lie group acts on a space, we have two derivatives to care about: +Lie Group Action +---------------- + +When a Lie group acts on a space, we have two derivatives to care about: * `group::act(g,t,Hg,Ht)`, if the space acted upon is a continuous differentiable manifold From 39ee5c5dcad132a0248795e190e72fa4361b4057 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 5 Dec 2014 21:18:53 +0000 Subject: [PATCH 031/199] A base class for Charts, no more ManifoldType trait... --- GTSAM-Concepts.md | 70 +++++++++++++++++++++++++++++++++-------------- 1 file changed, 50 insertions(+), 20 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 3e324c5cd..77c1621bf 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -38,10 +38,10 @@ Chart A given chart is implemented using a small class that defines the chart itself (from manifold to tangent space) and its inverse. * types: - * `Manifold`, a pointer back to the type + * `ManifoldType`, a pointer back to the type * valid expressions: - * `v = Chart::local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p* - * `p = Chart::retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v* + * `v = Chart::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p* + * `p = Chart::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v* For many differential manifolds, an obvious mapping is the `exponential map`, which 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: @@ -139,18 +139,54 @@ Unit tests heavily depend on the following two functions being defined for all t 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 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). +`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). Charts are done more conventionally, so we start there... + +Interfaces +---------- + +Because Charts are always written by the user (or automatically generated, see below for vector spaces), +we enforce the Chart concept using an abstract base class, acting as an interface: + +``` +#!c++ +template +struct Chart { + typedef T ManifoldType; + typedef typename traits::TangentVector::type TangentVector; + static TangentVector Local(const ManifoldType& p, const ManifoldType& q) {return Derived::local(p,q);} + static ManifoldType Retract(const ManifoldType& p, const TangentVector& v) {return Derived::retract(p,v);} + protected: + Chart(){ (void)&Local; (void)&Retract; } // enforce early instantiation. +} +``` + +The [CRTP](http://en.wikipedia.org/wiki/Curiously_recurring_template_pattern) and the protected constructor +automatically check for the existence of the methods in the Derived class, whenever a new Chart is created by + + struct MyChart : Chart { ... } Traits ------ -We will not use Eigen-style or STL-style traits, that define *many* properties at once. Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming, which return a single type or value for every trait. Some rationale/history can be found [here](http://www.boost.org/doc/libs/1_55_0/libs/type_traits/doc/html/boost_typetraits/background.html). +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. -Traits allow us to play with types that are outside GTSAM control, e.g., `Eigen::VectorXd`. However, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types, as well, rather than having to use traits internally. +We will not use Eigen-style or STL-style traits, that define *many* properties at once. +Rather, we use boost::mpl style meta-programming functions to facilitate meta-programming, +which return a single type or value for every trait. Some rationale/history can be +found [here](http://www.boost.org/doc/libs/1_55_0/libs/type_traits/doc/html/boost_typetraits/background.html). +as well. -Finally, note that not everything that makes a concept is defined by traits. For example, although a CHART type is supposed to have a `retract` function, there is no trait for this: rather, the +Note that not everything that makes a concept is defined by traits. Valid expressions such as group::compose are +defined simply as free functions. +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. The conventions for `gtsam::traits` are as follows: @@ -171,7 +207,7 @@ The conventions for `gtsam::traits` are as follows: * Functors: `gtsam::traits::someFunctor::type`, i.e., they are mixedCase starting with a lowercase letter and define a functor (i.e., no *type*). The functor itself should define a `result_type`. A contrived example - struct Point2::manhattan { + struct Point2::manhattan { typedef double result_type; Point2 p_; manhattan(const Point2& p) : p_(p) {} @@ -179,10 +215,9 @@ The conventions for `gtsam::traits` are as follows: return abs(p_.x()-q.x()) + abs(p_.y()-q.x()); } } - - template<> - gtsam::traits::manhattan : Point2::manhattan {} - + + template<> gtsam::traits::manhattan : Point2::manhattan {} + By *inherting* the trait from the functor, we can just use the [currying](http://en.wikipedia.org/wiki/Currying) style `gtsam::traits::manhattan℗(q)`. Note that, although technically a functor is a type, in spirit it is a free function and hence starts with a lowercase letter. * Tags: `gtsam::traits::some_category::type`, i.e., they are lower_case and define a *single* `type`, for example: @@ -261,8 +296,8 @@ An example of implementing a Manifold type is here: #!c++ // GTSAM type class Rot2 { - typedef Vector2 TangentVector; - class Chart { + typedef Vector2 TangentVector; // internal typedef, not required + class Chart : gtsam::Chart{ static TangentVector local(const Rot2& R1, const Rot2& R2); static Rot2 retract(const Rot2& R, const TangentVector& v); } @@ -286,11 +321,6 @@ An example of implementing a Manifold type is here: template<> struct defaultChart : Rot2::Chart {} - template<> - struct Manifold { - typedef Rot2 type; - } - }} ``` From 6f82458bee410b05e607d333619aded78b206704 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 10:17:35 +0100 Subject: [PATCH 032/199] Trying to firm things up by having a small unit test for cyclic groups. --- .cproject | 106 ++++++++++++++-------------- gtsam/base/concepts.h | 102 ++++++++++++++++---------- gtsam/geometry/tests/testCyclic.cpp | 67 ++++++++++++++++++ 3 files changed, 182 insertions(+), 93 deletions(-) create mode 100644 gtsam/geometry/tests/testCyclic.cpp diff --git a/.cproject b/.cproject index 617aa3795..f45c2d690 100644 --- a/.cproject +++ b/.cproject @@ -592,7 +592,6 @@ make - tests/testBayesTree.run true false @@ -600,7 +599,6 @@ make - testBinaryBayesNet.run true false @@ -648,7 +646,6 @@ make - testSymbolicBayesNet.run true false @@ -656,7 +653,6 @@ make - tests/testSymbolicFactor.run true false @@ -664,7 +660,6 @@ make - testSymbolicFactorGraph.run true false @@ -680,7 +675,6 @@ make - tests/testBayesTree true false @@ -1136,7 +1130,6 @@ make - testErrors.run true false @@ -1366,46 +1359,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1488,6 +1441,7 @@ make + testSimulated2DOriented.run true false @@ -1527,6 +1481,7 @@ make + testSimulated2D.run true false @@ -1534,6 +1489,7 @@ make + testSimulated3D.run true false @@ -1547,6 +1503,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1804,7 +1800,6 @@ cpack - -G DEB true false @@ -1812,7 +1807,6 @@ cpack - -G RPM true false @@ -1820,7 +1814,6 @@ cpack - -G TGZ true false @@ -1828,7 +1821,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2185,6 +2177,14 @@ true true + + make + -j4 + testCyclic.run + true + true + true + make -j2 @@ -2683,7 +2683,6 @@ make - testGraph.run true false @@ -2691,7 +2690,6 @@ make - testJunctionTree.run true false @@ -2699,7 +2697,6 @@ make - testSymbolicBayesNetB.run true false @@ -3251,6 +3248,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index cc86d98ac..967a91707 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -1,57 +1,62 @@ /* * concepts.h * - * Created on: Dec 4, 2014 - * Author: mike bosse + * @data Dec 4, 2014 + * @author Mike Bosse + * @author Frank Dellaert */ -#ifndef CONCEPTS_H_ -#define CONCEPTS_H_ +#pragma once -#include "manifold.h" -#include "chart.h" -#include +//#include "manifold.h" +//#include "chart.h" +#include +#include +#include namespace gtsam { namespace traits { -template -struct TangentVector { - //typedef XXX type; -}; - -// used to identify the manifold associated with a chart -template -struct Manifold { - //typedef XXX type; -}; - -template -struct DefaultChart { - //typedef XXX type; -}; - +/** + * @name Algebraic Structure Traits + * @brief Associate a unique tag with each of the main GTSAM concepts + */ +//@{ template -struct structure {}; // specializations should be derived from one of the following tags +struct structure_category {}; // specializations should be derived from one of the following tags +//@} + +/** + * @name Algebraic Structure Tags + * @brief Possible values for traits::structure_category::type + */ +//@{ struct manifold_tag {}; struct group_tag {}; struct lie_group_tag : public manifold_tag, public group_tag {}; struct vector_space_tag : public lie_group_tag {}; - -template -struct group_flavor {}; -struct additive_group_tag {}; -struct multiplicative_group_tag {}; +//@} } // namespace traits +namespace traits { + +/** @name Manifold Traits */ +//@{ +template struct TangentVector; +template struct DefaultChart; +//@} + +} // namespace traits + +/* template class ManifoldConcept { public: typedef T Manifold; - typedef traits::TangentVector::type TangentVector; - typedef traits::DefaultChart::type DefaultChart; + typedef typename traits::TangentVector::type TangentVector; + typedef typename traits::DefaultChart::type DefaultChart; static const size_t dim = traits::dimension::value; BOOST_CONCEPT_USAGE(ManifoldConcept) { @@ -81,6 +86,23 @@ class ChartConcept { TangentVector v; }; +*/ + +namespace traits { + +/** @name Group Traits */ +//@{ +template struct identity {}; +template struct group_flavor {}; +//@} + +/** @name Group Flavor Tags */ +//@{ +struct additive_group_tag {}; +struct multiplicative_group_tag {}; +//@} + +} // namespace traits template class GroupConcept { @@ -89,23 +111,24 @@ class GroupConcept { static const Group identity = traits::identity::value; BOOST_CONCEPT_USAGE(GroupConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); + BOOST_STATIC_ASSERT(boost::is_base_of::type>::value ); Group ip = inverse(p); Group pq = compose(p, q); Group d = between(p, q); - bool test = equal(p, q); - bool test2 = operator_usage(p, q, traits::group_flavor); + test = equal(p, q); + test2 = operator_usage(p, q, traits::group_flavor::type); } bool check_invariants(const Group& a, const Group& b) { return (equal(compose(a, inverse(a)), identity)) && (equal(between(a, b), compose(inverse(a), b))) && (equal(compose(a, between(a, b)), b)) - && operator_usage(a, b, traits::group_flavor); + && operator_usage(a, b, traits::group_flavor::type); } private: Group p,q; + bool test, test2; bool operator_usage(const Group& a, const Group& b, const traits::multiplicative_group_tag&) { return equal(compose(a, b), a*b); @@ -116,7 +139,7 @@ class GroupConcept { } }; - +/* template class LieGroupConcept : public GroupConcept, public ManifoldConcept { @@ -127,8 +150,8 @@ class LieGroupConcept : public GroupConcept, public ManifoldConcept { template class VectorSpaceConcept : public LieGroupConcept { - typedef traits::DefaultChart::type Chart; - typedef GroupConcept::identity identity; + typedef typename traits::DefaultChart::type Chart; + typedef typename GroupConcept::identity identity; BOOST_CONCEPT_USAGE(VectorSpaceConcept) { BOOST_STATIC_ASSERT(boost::is_base_of >); @@ -148,7 +171,8 @@ class VectorSpaceConcept : public LieGroupConcept { private: V p,q,r; }; +*/ } // namespace gtsam -#endif /* CONCEPTS_H_ */ + diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp new file mode 100644 index 000000000..f15654b2e --- /dev/null +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -0,0 +1,67 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include + +namespace gtsam { + +template +class Cyclic { + size_t i_; +public: + static const Cyclic Identity = Cyclic(0); + Cyclic(size_t i) : + i_(i) { + } +}; + +namespace traits { +template struct identity > { + static const Cyclic value = Cyclic::Identity; + typedef Cyclic value_type; +}; +template struct group_flavor > { + typedef additive_group_tag type; +}; +} // \namespace traits + +} // \namespace gtsam + +//#include +#include +#include + +using namespace std; +using namespace gtsam; + +BOOST_CONCEPT_ASSERT((GroupConcept >)); + +/* ************************************************************************* */ +TEST(Cyclic, Constructor) { +Cyclic<6> g(0); +// EXPECT(assert_equal(p1, p2)); +// EXPECT_LONGS_EQUAL(2,offset2.size()); +} + +/* ************************************************************************* */ +int main() { +TestResult tr; +return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 71e77b8c879b5884cc19eb3229c377b34f74b91d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 10:58:16 +0100 Subject: [PATCH 033/199] Compiles --- gtsam/geometry/tests/testCyclic.cpp | 61 ++++++++++++++++++++++------- 1 file changed, 47 insertions(+), 14 deletions(-) diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index f15654b2e..bdf0c3e98 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testCyclic.cpp - * @brief Unit tests for cyclic group + * @file Cyclic.h + * @brief Cyclic group, can act on 2D vector spaces * @author Frank Dellaert **/ @@ -22,17 +22,21 @@ namespace gtsam { template class Cyclic { - size_t i_; + size_t i_; ///< we just use an unsigned int public: - static const Cyclic Identity = Cyclic(0); + /// Constructor Cyclic(size_t i) : i_(i) { } + /// Cast to size_t + operator size_t() const { + return i_; + } }; namespace traits { template struct identity > { - static const Cyclic value = Cyclic::Identity; + static const Cyclic value; typedef Cyclic value_type; }; template struct group_flavor > { @@ -42,6 +46,27 @@ template struct group_flavor > { } // \namespace gtsam +/** + * @file Cyclic.cpp + * @brief Cyclic group implementation + * @author Frank Dellaert + **/ + +namespace gtsam { + +namespace traits { +template +const Cyclic identity >::value = Cyclic(0); +} // \namespace traits + +} // \namespace gtsam + +/** + * @file testCyclic.cpp + * @brief Unit tests for cyclic group + * @author Frank Dellaert + **/ + //#include #include #include @@ -49,19 +74,27 @@ template struct group_flavor > { using namespace std; using namespace gtsam; -BOOST_CONCEPT_ASSERT((GroupConcept >)); +typedef Cyclic<6> G; -/* ************************************************************************* */ -TEST(Cyclic, Constructor) { -Cyclic<6> g(0); +//****************************************************************************** +TEST(Cyclic, Concept) { + EXPECT_LONGS_EQUAL(0,traits::identity::value); + //BOOST_CONCEPT_ASSERT((GroupConcept >)); // EXPECT(assert_equal(p1, p2)); // EXPECT_LONGS_EQUAL(2,offset2.size()); } -/* ************************************************************************* */ -int main() { -TestResult tr; -return TestRegistry::runAllTests(tr); +//****************************************************************************** +TEST(Cyclic, Constructor) { + G g(0); +// EXPECT(assert_equal(p1, p2)); +// EXPECT_LONGS_EQUAL(2,offset2.size()); } -/* ************************************************************************* */ + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** From 07ef30039a5363fe43a617f9a36b18a4bab85d3f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 11:17:54 +0100 Subject: [PATCH 034/199] structure_tag works --- gtsam/base/concepts.h | 55 +++++++++++++++++------------ gtsam/geometry/tests/testCyclic.cpp | 13 ++++--- 2 files changed, 38 insertions(+), 30 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 967a91707..393bb69b1 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -24,7 +24,7 @@ namespace traits { */ //@{ template -struct structure_category {}; // specializations should be derived from one of the following tags +struct structure_category; // specializations should be derived from one of the following tags //@} /** @@ -92,8 +92,8 @@ namespace traits { /** @name Group Traits */ //@{ -template struct identity {}; -template struct group_flavor {}; +template struct identity; +template struct group_flavor; //@} /** @name Group Flavor Tags */ @@ -104,41 +104,50 @@ struct multiplicative_group_tag {}; } // namespace traits +/** + * Group Concept + */ template -class GroupConcept { - public: - typedef G Group; - static const Group identity = traits::identity::value; +class Group { +public: - BOOST_CONCEPT_USAGE(GroupConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of::type>::value ); - Group ip = inverse(p); - Group pq = compose(p, q); - Group d = between(p, q); - test = equal(p, q); - test2 = operator_usage(p, q, traits::group_flavor::type); + typedef typename traits::identity::value_type identity_value_type; + typedef typename traits::group_flavor::type group_flavor_tag; + typedef typename traits::structure_category::type structure_category_tag; + + BOOST_CONCEPT_USAGE(Group) { + e = traits::identity::value; +// BOOST_STATIC_ASSERT(boost::is_base_of::type>::value ); +// G ip = inverse(p); +// G pq = compose(p, q); +// G d = between(p, q); +// test = equal(p, q); +// test2 = operator_usage(p, q, traits::group_flavor::type); } - bool check_invariants(const Group& a, const Group& b) { - return (equal(compose(a, inverse(a)), identity)) + bool check_invariants(const G& a, const G& b) { + group_flavor_tag group_flavor; + return (equal(compose(a, inverse(a)), e)) && (equal(between(a, b), compose(inverse(a), b))) && (equal(compose(a, between(a, b)), b)) - && operator_usage(a, b, traits::group_flavor::type); + && operator_usage(a, b, group_flavor); } - private: - Group p,q; +private: + G p, q, e; bool test, test2; - bool operator_usage(const Group& a, const Group& b, const traits::multiplicative_group_tag&) { - return equal(compose(a, b), a*b); + bool operator_usage(const G& a, const G& b, + traits::multiplicative_group_tag) { + return equal(compose(a, b), a * b); } - bool operator_usage(const Group& a, const Group& b, const traits::additive_group_tag&) { - return equal(compose(a, b), a+b); + bool operator_usage(const G& a, const G& b, traits::additive_group_tag) { + return equal(compose(a, b), a + b); } }; + /* template class LieGroupConcept : public GroupConcept, public ManifoldConcept { diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index bdf0c3e98..8ea678a0f 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -11,7 +11,7 @@ /** * @file Cyclic.h - * @brief Cyclic group, can act on 2D vector spaces + * @brief Cyclic group, i.e., the integers modulo N * @author Frank Dellaert **/ @@ -35,6 +35,9 @@ public: }; namespace traits { +template struct structure_category > { + typedef group_tag type; +}; template struct identity > { static const Cyclic value; typedef Cyclic value_type; @@ -74,21 +77,17 @@ const Cyclic identity >::value = Cyclic(0); using namespace std; using namespace gtsam; -typedef Cyclic<6> G; +typedef Cyclic<6> G; // Let's use the cyclic group of order 6 //****************************************************************************** TEST(Cyclic, Concept) { EXPECT_LONGS_EQUAL(0,traits::identity::value); - //BOOST_CONCEPT_ASSERT((GroupConcept >)); -// EXPECT(assert_equal(p1, p2)); -// EXPECT_LONGS_EQUAL(2,offset2.size()); + BOOST_CONCEPT_ASSERT((Group)); } //****************************************************************************** TEST(Cyclic, Constructor) { G g(0); -// EXPECT(assert_equal(p1, p2)); -// EXPECT_LONGS_EQUAL(2,offset2.size()); } //****************************************************************************** From ddeb7649121942ab803ce40cba3cf2ee3b17fedf Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 11:37:14 +0100 Subject: [PATCH 035/199] group::traits, and group::compose --- gtsam/base/concepts.h | 172 +++++++++++++++------------- gtsam/geometry/tests/testCyclic.cpp | 19 ++- 2 files changed, 109 insertions(+), 82 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 393bb69b1..6910518da 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -23,8 +23,9 @@ namespace traits { * @brief Associate a unique tag with each of the main GTSAM concepts */ //@{ -template -struct structure_category; // specializations should be derived from one of the following tags +template +struct structure_category; +// specializations should be derived from one of the following tags //@} /** @@ -32,97 +33,111 @@ struct structure_category; // specializations should be derived from one of the * @brief Possible values for traits::structure_category::type */ //@{ -struct manifold_tag {}; -struct group_tag {}; -struct lie_group_tag : public manifold_tag, public group_tag {}; -struct vector_space_tag : public lie_group_tag {}; +struct manifold_tag { +}; +struct group_tag { +}; +struct lie_group_tag: public manifold_tag, public group_tag { +}; +struct vector_space_tag: public lie_group_tag { +}; //@} -} // namespace traits +}// namespace traits namespace traits { /** @name Manifold Traits */ //@{ -template struct TangentVector; -template struct DefaultChart; +template struct TangentVector; +template struct DefaultChart; //@} -} // namespace traits +}// namespace traits /* -template -class ManifoldConcept { + template + class ManifoldConcept { public: - typedef T Manifold; - typedef typename traits::TangentVector::type TangentVector; - typedef typename traits::DefaultChart::type DefaultChart; - static const size_t dim = traits::dimension::value; + typedef T Manifold; + typedef typename traits::TangentVector::type TangentVector; + typedef typename traits::DefaultChart::type DefaultChart; + static const size_t dim = traits::dimension::value; - BOOST_CONCEPT_USAGE(ManifoldConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - // no direct usage for manifold since most usage is through a chart - } + BOOST_CONCEPT_USAGE(ManifoldConcept) { + BOOST_STATIC_ASSERT(boost::is_base_of >); + BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); + // no direct usage for manifold since most usage is through a chart + } private: - Manifold p; - TangentVector v; -}; + Manifold p; + TangentVector v; + }; -template -class ChartConcept { + template + class ChartConcept { public: - typedef C Chart; - typedef typename traits::Manifold::type Manifold; - typedef typename traits::TangentVector::type TangentVector; + typedef C Chart; + typedef typename traits::Manifold::type Manifold; + typedef typename traits::TangentVector::type TangentVector; - BOOST_CONCEPT_USAGE(ChartConcept) { - v = Chart::local(p,q); // returns local coordinates of q w.r.t. origin p - q = Chart::retract(p,v); // returns retracted update of p with v - } + BOOST_CONCEPT_USAGE(ChartConcept) { + v = Chart::local(p,q); // returns local coordinates of q w.r.t. origin p + q = Chart::retract(p,v); // returns retracted update of p with v + } private: - Manifold p,q; - TangentVector v; + Manifold p,q; + TangentVector v; -}; -*/ + }; + */ + +namespace group { + +template +G compose(const G&g, const G& h); namespace traits { /** @name Group Traits */ //@{ -template struct identity; -template struct group_flavor; +template struct identity; +template struct flavor; //@} /** @name Group Flavor Tags */ //@{ -struct additive_group_tag {}; -struct multiplicative_group_tag {}; +struct additive_tag { +}; +struct multiplicative_tag { +}; //@} -} // namespace traits +} // \ namespace traits +} // \ namespace group /** * Group Concept */ -template +template class Group { public: - typedef typename traits::identity::value_type identity_value_type; - typedef typename traits::group_flavor::type group_flavor_tag; typedef typename traits::structure_category::type structure_category_tag; + typedef typename group::traits::identity::value_type identity_value_type; + typedef typename group::traits::flavor::type group_flavor_tag; BOOST_CONCEPT_USAGE(Group) { - e = traits::identity::value; -// BOOST_STATIC_ASSERT(boost::is_base_of::type>::value ); + using group::compose; + BOOST_STATIC_ASSERT( + boost::is_base_of::value); + e = group::traits::identity::value; + pq = compose(p, q); // G ip = inverse(p); -// G pq = compose(p, q); // G d = between(p, q); // test = equal(p, q); -// test2 = operator_usage(p, q, traits::group_flavor::type); +// test2 = operator_usage(p, q, group::traits::_flavor::type); } bool check_invariants(const G& a, const G& b) { @@ -134,54 +149,53 @@ public: } private: - G p, q, e; + G e, p, q, pq, ip, d; bool test, test2; bool operator_usage(const G& a, const G& b, - traits::multiplicative_group_tag) { + group::traits::multiplicative_tag) { return equal(compose(a, b), a * b); } - bool operator_usage(const G& a, const G& b, traits::additive_group_tag) { + bool operator_usage(const G& a, const G& b, group::traits::additive_tag) { return equal(compose(a, b), a + b); } }; /* -template -class LieGroupConcept : public GroupConcept, public ManifoldConcept { + template + class LieGroupConcept : public GroupConcept, public ManifoldConcept { - BOOST_CONCEPT_USAGE(LieGroupConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - } -}; + BOOST_CONCEPT_USAGE(LieGroupConcept) { + BOOST_STATIC_ASSERT(boost::is_base_of >); + } + }; -template -class VectorSpaceConcept : public LieGroupConcept { - typedef typename traits::DefaultChart::type Chart; - typedef typename GroupConcept::identity identity; + template + class VectorSpaceConcept : public LieGroupConcept { + typedef typename traits::DefaultChart::type Chart; + typedef typename GroupConcept::identity identity; - BOOST_CONCEPT_USAGE(VectorSpaceConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - r = p+q; - r = -p; - r = p-q; - } + BOOST_CONCEPT_USAGE(VectorSpaceConcept) { + BOOST_STATIC_ASSERT(boost::is_base_of >); + r = p+q; + r = -p; + r = p-q; + } - bool check_invariants(const V& a, const V& b) { - return equal(compose(a, b), a+b) - && equal(inverse(a), -a) - && equal(between(a, b), b-a) - && equal(Chart::retract(a, b), a+b) - && equal(Chart::local(a, b), b-a); - } + bool check_invariants(const V& a, const V& b) { + return equal(compose(a, b), a+b) + && equal(inverse(a), -a) + && equal(between(a, b), b-a) + && equal(Chart::retract(a, b), a+b) + && equal(Chart::local(a, b), b-a); + } private: - V p,q,r; -}; -*/ + V p,q,r; + }; + */ } // namespace gtsam - diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 8ea678a0f..7bc19a2b7 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -35,17 +35,28 @@ public: }; namespace traits { +/// Define Cyclic to be a model of the Group concept template struct structure_category > { typedef group_tag type; }; +} // \namespace traits + +namespace group { +template +Cyclic compose(const Cyclic&g, const Cyclic& h); + +namespace traits { +/// Define the trait that specifies Cyclic's identity element template struct identity > { static const Cyclic value; typedef Cyclic value_type; }; -template struct group_flavor > { - typedef additive_group_tag type; +/// Define the trait that asserts Cyclic is an additive group +template struct flavor > { + typedef additive_tag type; }; } // \namespace traits +} // \namespace group } // \namespace gtsam @@ -57,10 +68,12 @@ template struct group_flavor > { namespace gtsam { +namespace group { namespace traits { template const Cyclic identity >::value = Cyclic(0); } // \namespace traits +} // \namespace group } // \namespace gtsam @@ -81,7 +94,7 @@ typedef Cyclic<6> G; // Let's use the cyclic group of order 6 //****************************************************************************** TEST(Cyclic, Concept) { - EXPECT_LONGS_EQUAL(0,traits::identity::value); + EXPECT_LONGS_EQUAL(0, group::traits::identity::value); BOOST_CONCEPT_ASSERT((Group)); } From 5407232e367a2d25a9cdcbcd1fa2308e6ce418d4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 11:51:44 +0100 Subject: [PATCH 036/199] between and inverse, as well as tag dispatching --- gtsam/base/concepts.h | 34 +++++++++++++++++------------ gtsam/geometry/tests/testCyclic.cpp | 6 +++++ 2 files changed, 26 insertions(+), 14 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 6910518da..d3f2fe87c 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -95,8 +95,12 @@ template struct DefaultChart; namespace group { -template -G compose(const G&g, const G& h); +/** @name Free functions any Group needs to define */ +//@{ +template G compose(const G&g, const G& h); +template G between(const G&g, const G& h); +template G inverse(const G&g); +//@} namespace traits { @@ -126,39 +130,41 @@ public: typedef typename traits::structure_category::type structure_category_tag; typedef typename group::traits::identity::value_type identity_value_type; - typedef typename group::traits::flavor::type group_flavor_tag; + typedef typename group::traits::flavor::type flavor_tag; BOOST_CONCEPT_USAGE(Group) { using group::compose; + using group::between; + using group::inverse; BOOST_STATIC_ASSERT( boost::is_base_of::value); e = group::traits::identity::value; - pq = compose(p, q); -// G ip = inverse(p); -// G d = between(p, q); -// test = equal(p, q); -// test2 = operator_usage(p, q, group::traits::_flavor::type); + d = compose(g, h); + d = between(g, h); + ig = inverse(g); + test = operator_usage(g, h, flavor); +// test2 = equal(g, h); } bool check_invariants(const G& a, const G& b) { - group_flavor_tag group_flavor; return (equal(compose(a, inverse(a)), e)) && (equal(between(a, b), compose(inverse(a), b))) && (equal(compose(a, between(a, b)), b)) - && operator_usage(a, b, group_flavor); + && operator_usage(a, b, flavor); } private: - G e, p, q, pq, ip, d; + flavor_tag flavor; + G e, g, h, gh, ig, d; bool test, test2; bool operator_usage(const G& a, const G& b, group::traits::multiplicative_tag) { - return equal(compose(a, b), a * b); + return group::compose(a, b) == a * b; } bool operator_usage(const G& a, const G& b, group::traits::additive_tag) { - return equal(compose(a, b), a + b); + return group::compose(a, b) == a + b; } }; @@ -193,7 +199,7 @@ private: } private: - V p,q,r; + V g,q,r; }; */ diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 7bc19a2b7..882d8fb1c 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -45,6 +45,12 @@ namespace group { template Cyclic compose(const Cyclic&g, const Cyclic& h); +template +Cyclic between(const Cyclic&g, const Cyclic& h); + +template +Cyclic inverse(const Cyclic&g); + namespace traits { /// Define the trait that specifies Cyclic's identity element template struct identity > { From 6c2b28aa8b2a1f1b268fe15802108b9a3009591d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 12:50:40 +0100 Subject: [PATCH 037/199] between tested --- gtsam/base/concepts.h | 18 +++++++------ gtsam/geometry/tests/testCyclic.cpp | 39 +++++++++++++++++++++++++---- 2 files changed, 45 insertions(+), 12 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index d3f2fe87c..9011c759b 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -143,15 +143,19 @@ public: d = between(g, h); ig = inverse(g); test = operator_usage(g, h, flavor); -// test2 = equal(g, h); } - bool check_invariants(const G& a, const G& b) { - return (equal(compose(a, inverse(a)), e)) - && (equal(between(a, b), compose(inverse(a), b))) - && (equal(compose(a, between(a, b)), b)) - && operator_usage(a, b, flavor); - } +// TODO: these all require default constructors :-( +// Also, requires equal which is not required of a group +// Group():e(group::traits::identity::value) { +// } +// +// bool check_invariants(const G& a, const G& b) { +// return (equal(compose(a, inverse(a)), e)) +// && (equal(between(a, b), compose(inverse(a), b))) +// && (equal(compose(a, between(a, b)), b)) +// && operator_usage(a, b, flavor); +// } private: flavor_tag flavor; diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 882d8fb1c..f46b85511 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -32,6 +32,10 @@ public: operator size_t() const { return i_; } + /// Addition modulo N + Cyclic operator+(const Cyclic& h) const { + return (i_+h.i_) % N; + } }; namespace traits { @@ -42,14 +46,21 @@ template struct structure_category > { } // \namespace traits namespace group { -template -Cyclic compose(const Cyclic&g, const Cyclic& h); template -Cyclic between(const Cyclic&g, const Cyclic& h); +Cyclic compose(const Cyclic&g, const Cyclic& h) { + return g + h; +} template -Cyclic inverse(const Cyclic&g); +Cyclic between(const Cyclic&g, const Cyclic& h) { + return h - g; +} + +template +Cyclic inverse(const Cyclic&g) { + return -g; +} namespace traits { /// Define the trait that specifies Cyclic's identity element @@ -100,8 +111,10 @@ typedef Cyclic<6> G; // Let's use the cyclic group of order 6 //****************************************************************************** TEST(Cyclic, Concept) { - EXPECT_LONGS_EQUAL(0, group::traits::identity::value); BOOST_CONCEPT_ASSERT((Group)); + EXPECT_LONGS_EQUAL(0, group::traits::identity::value); + G g(2), h(3); + // EXPECT(Group().check_invariants(g,h)) } //****************************************************************************** @@ -109,6 +122,22 @@ TEST(Cyclic, Constructor) { G g(0); } +//****************************************************************************** +TEST(Cyclic, Compose) { + G e(0), g(2), h(3); + EXPECT_LONGS_EQUAL(5, group::compose(g,h)); + EXPECT_LONGS_EQUAL(0, group::compose(h,h)); + EXPECT_LONGS_EQUAL(3, group::compose(h,e)); +} + +//****************************************************************************** +TEST(Cyclic, Between) { + G g(2), h(3); + EXPECT_LONGS_EQUAL(1, group::between(g,h)); + EXPECT_LONGS_EQUAL(0, group::between(g,g)); + EXPECT_LONGS_EQUAL(0, group::between(h,h)); +} + //****************************************************************************** int main() { TestResult tr; From 4d2a0aefc103070e3cc76af3fe27129dad680241 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 12:59:35 +0100 Subject: [PATCH 038/199] Group checks out ! --- gtsam/geometry/tests/testCyclic.cpp | 52 ++++++++++++++++++++++++----- 1 file changed, 44 insertions(+), 8 deletions(-) diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index f46b85511..61c700c8a 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -36,6 +36,14 @@ public: 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; + } }; namespace traits { @@ -124,18 +132,46 @@ TEST(Cyclic, Constructor) { //****************************************************************************** TEST(Cyclic, Compose) { - G e(0), g(2), h(3); - EXPECT_LONGS_EQUAL(5, group::compose(g,h)); - EXPECT_LONGS_EQUAL(0, group::compose(h,h)); - EXPECT_LONGS_EQUAL(3, group::compose(h,e)); + EXPECT_LONGS_EQUAL(0, group::compose(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, group::compose(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, group::compose(G(0),G(2))); + EXPECT_LONGS_EQUAL(3, group::compose(G(0),G(3))); + EXPECT_LONGS_EQUAL(4, group::compose(G(0),G(4))); + EXPECT_LONGS_EQUAL(5, group::compose(G(0),G(5))); + + EXPECT_LONGS_EQUAL(2, group::compose(G(2),G(0))); + EXPECT_LONGS_EQUAL(3, group::compose(G(2),G(1))); + EXPECT_LONGS_EQUAL(4, group::compose(G(2),G(2))); + EXPECT_LONGS_EQUAL(5, group::compose(G(2),G(3))); + EXPECT_LONGS_EQUAL(0, group::compose(G(2),G(4))); + EXPECT_LONGS_EQUAL(1, group::compose(G(2),G(5))); } //****************************************************************************** TEST(Cyclic, Between) { - G g(2), h(3); - EXPECT_LONGS_EQUAL(1, group::between(g,h)); - EXPECT_LONGS_EQUAL(0, group::between(g,g)); - EXPECT_LONGS_EQUAL(0, group::between(h,h)); + EXPECT_LONGS_EQUAL(0, group::between(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, group::between(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, group::between(G(0),G(2))); + EXPECT_LONGS_EQUAL(3, group::between(G(0),G(3))); + EXPECT_LONGS_EQUAL(4, group::between(G(0),G(4))); + EXPECT_LONGS_EQUAL(5, group::between(G(0),G(5))); + + EXPECT_LONGS_EQUAL(4, group::between(G(2),G(0))); + EXPECT_LONGS_EQUAL(5, group::between(G(2),G(1))); + EXPECT_LONGS_EQUAL(0, group::between(G(2),G(2))); + EXPECT_LONGS_EQUAL(1, group::between(G(2),G(3))); + EXPECT_LONGS_EQUAL(2, group::between(G(2),G(4))); + EXPECT_LONGS_EQUAL(3, group::between(G(2),G(5))); +} + +//****************************************************************************** +TEST(Cyclic, Ivnverse) { + EXPECT_LONGS_EQUAL(0, group::inverse(G(0))); + EXPECT_LONGS_EQUAL(5, group::inverse(G(1))); + EXPECT_LONGS_EQUAL(4, group::inverse(G(2))); + EXPECT_LONGS_EQUAL(3, group::inverse(G(3))); + EXPECT_LONGS_EQUAL(2, group::inverse(G(4))); + EXPECT_LONGS_EQUAL(1, group::inverse(G(5))); } //****************************************************************************** From 614bfeb9d39a375a5a16904214772b9cf15684b7 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 13:11:56 +0100 Subject: [PATCH 039/199] Moved to headers --- gtsam/geometry/Cyclic.cpp | 27 ++++++++ gtsam/geometry/Cyclic.h | 89 +++++++++++++++++++++++++++ gtsam/geometry/tests/testCyclic.cpp | 95 +---------------------------- 3 files changed, 117 insertions(+), 94 deletions(-) create mode 100644 gtsam/geometry/Cyclic.cpp create mode 100644 gtsam/geometry/Cyclic.h diff --git a/gtsam/geometry/Cyclic.cpp b/gtsam/geometry/Cyclic.cpp new file mode 100644 index 000000000..2b14ec6ba --- /dev/null +++ b/gtsam/geometry/Cyclic.cpp @@ -0,0 +1,27 @@ +/* ---------------------------------------------------------------------------- + + * 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.cpp + * @brief Cyclic group implementation + * @author Frank Dellaert + **/ + +#include + +namespace gtsam { +namespace group { +namespace traits { + +} // \namespace traits +} // \namespace group +} // \namespace gtsam + diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h new file mode 100644 index 000000000..85602d12d --- /dev/null +++ b/gtsam/geometry/Cyclic.h @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cyclic.h + * @brief Cyclic group, i.e., the integers modulo N + * @author Frank Dellaert + **/ + +#include +#include + +namespace gtsam { + +template +class Cyclic { + size_t i_; ///< we just use an unsigned int +public: + /// Constructor + Cyclic(size_t i) : + i_(i) { + } + /// 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; + } +}; + +namespace traits { +/// Define Cyclic to be a model of the Group concept +template struct structure_category > { + typedef group_tag type; +}; +} // \namespace gtsam::traits + +namespace group { + +template +Cyclic compose(const Cyclic&g, const Cyclic& h) { + return g + h; +} + +template +Cyclic between(const Cyclic&g, const Cyclic& h) { + return h - g; +} + +template +Cyclic inverse(const Cyclic&g) { + return -g; +} + +namespace traits { + +/// Define the trait that specifies Cyclic's identity element +template struct identity > { + static const Cyclic value = Cyclic(0); + typedef Cyclic value_type; +}; + +/// Define the trait that asserts Cyclic is an additive group +template struct flavor > { + typedef additive_tag type; +}; + +} // \namespace gtsam::group::traits +} // \namespace gtsam::group +} // \namespace gtsam + diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 61c700c8a..00ea2c853 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -9,106 +9,13 @@ * -------------------------------------------------------------------------- */ -/** - * @file Cyclic.h - * @brief Cyclic group, i.e., the integers modulo N - * @author Frank Dellaert - **/ - -#include -#include - -namespace gtsam { - -template -class Cyclic { - size_t i_; ///< we just use an unsigned int -public: - /// Constructor - Cyclic(size_t i) : - i_(i) { - } - /// 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; - } -}; - -namespace traits { -/// Define Cyclic to be a model of the Group concept -template struct structure_category > { - typedef group_tag type; -}; -} // \namespace traits - -namespace group { - -template -Cyclic compose(const Cyclic&g, const Cyclic& h) { - return g + h; -} - -template -Cyclic between(const Cyclic&g, const Cyclic& h) { - return h - g; -} - -template -Cyclic inverse(const Cyclic&g) { - return -g; -} - -namespace traits { -/// Define the trait that specifies Cyclic's identity element -template struct identity > { - static const Cyclic value; - typedef Cyclic value_type; -}; -/// Define the trait that asserts Cyclic is an additive group -template struct flavor > { - typedef additive_tag type; -}; -} // \namespace traits -} // \namespace group - -} // \namespace gtsam - -/** - * @file Cyclic.cpp - * @brief Cyclic group implementation - * @author Frank Dellaert - **/ - -namespace gtsam { - -namespace group { -namespace traits { -template -const Cyclic identity >::value = Cyclic(0); -} // \namespace traits -} // \namespace group - -} // \namespace gtsam - /** * @file testCyclic.cpp * @brief Unit tests for cyclic group * @author Frank Dellaert **/ -//#include +#include #include #include From d1fec1a90d15c45103a369e77835e772162f7e37 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 13:17:44 +0100 Subject: [PATCH 040/199] Reverted moste changes in .cproject --- .cproject | 97 ++++++++++++++++++++++++++++++------------------------- 1 file changed, 53 insertions(+), 44 deletions(-) diff --git a/.cproject b/.cproject index f45c2d690..0598c8471 100644 --- a/.cproject +++ b/.cproject @@ -599,6 +599,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +647,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +655,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +663,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +679,7 @@ make + tests/testBayesTree true false @@ -1130,6 +1135,7 @@ make + testErrors.run true false @@ -1359,6 +1365,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1441,7 +1487,6 @@ make - testSimulated2DOriented.run true false @@ -1481,7 +1526,6 @@ make - testSimulated2D.run true false @@ -1489,7 +1533,6 @@ make - testSimulated3D.run true false @@ -1503,46 +1546,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1800,6 +1803,7 @@ cpack + -G DEB true false @@ -1807,6 +1811,7 @@ cpack + -G RPM true false @@ -1814,6 +1819,7 @@ cpack + -G TGZ true false @@ -1821,6 +1827,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2683,6 +2690,7 @@ make + testGraph.run true false @@ -2690,6 +2698,7 @@ make + testJunctionTree.run true false @@ -2697,6 +2706,7 @@ make + testSymbolicBayesNetB.run true false @@ -3248,7 +3258,6 @@ make - tests/testGaussianISAM2 true false From 491fd91af69a414f468811d056304ee80788c3d5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 14:28:39 +0100 Subject: [PATCH 041/199] Fixed compile problem. --- gtsam/geometry/Cyclic.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 85602d12d..71c7cbc4a 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -74,10 +74,13 @@ namespace traits { /// Define the trait that specifies Cyclic's identity element template struct identity > { - static const Cyclic value = Cyclic(0); + static const Cyclic value; typedef Cyclic value_type; }; +template +const Cyclic identity >::value = Cyclic(0); + /// Define the trait that asserts Cyclic is an additive group template struct flavor > { typedef additive_tag type; From e1c1d788c0f028ec8f98428ff50bb5146e7097e4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sat, 6 Dec 2014 18:01:13 +0100 Subject: [PATCH 042/199] Quaternions are a group (test compiles, at least) --- .cproject | 106 +++++++++++----------- gtsam/base/concepts.h | 5 +- gtsam/geometry/tests/testQuaternion.cpp | 113 ++++++++++++++++++++++++ 3 files changed, 169 insertions(+), 55 deletions(-) create mode 100644 gtsam/geometry/tests/testQuaternion.cpp diff --git a/.cproject b/.cproject index 0598c8471..57611d5b8 100644 --- a/.cproject +++ b/.cproject @@ -592,6 +592,7 @@ make + tests/testBayesTree.run true false @@ -599,7 +600,6 @@ make - testBinaryBayesNet.run true false @@ -647,7 +647,6 @@ make - testSymbolicBayesNet.run true false @@ -655,7 +654,6 @@ make - tests/testSymbolicFactor.run true false @@ -663,7 +661,6 @@ make - testSymbolicFactorGraph.run true false @@ -679,7 +676,6 @@ make - tests/testBayesTree true false @@ -1135,7 +1131,6 @@ make - testErrors.run true false @@ -1365,46 +1360,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1487,6 +1442,7 @@ make + testSimulated2DOriented.run true false @@ -1526,6 +1482,7 @@ make + testSimulated2D.run true false @@ -1533,6 +1490,7 @@ make + testSimulated3D.run true false @@ -1546,6 +1504,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1803,7 +1801,6 @@ cpack - -G DEB true false @@ -1811,7 +1808,6 @@ cpack - -G RPM true false @@ -1819,7 +1815,6 @@ cpack - -G TGZ true false @@ -1827,7 +1822,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2192,6 +2186,14 @@ true true + + make + -j4 + testQuaternion.run + true + true + true + make -j2 @@ -2690,7 +2692,6 @@ make - testGraph.run true false @@ -2698,7 +2699,6 @@ make - testJunctionTree.run true false @@ -2706,7 +2706,6 @@ make - testSymbolicBayesNetB.run true false @@ -3258,6 +3257,7 @@ make + tests/testGaussianISAM2 true false diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 9011c759b..dc4fca036 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -10,6 +10,7 @@ //#include "manifold.h" //#include "chart.h" +#include #include #include #include @@ -164,8 +165,8 @@ private: bool operator_usage(const G& a, const G& b, group::traits::multiplicative_tag) { - return group::compose(a, b) == a * b; - +// return group::compose(a, b) == a * b; + return true; } bool operator_usage(const G& a, const G& b, group::traits::additive_tag) { return group::compose(a, b) == a + b; diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp new file mode 100644 index 000000000..a35576259 --- /dev/null +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -0,0 +1,113 @@ +/* ---------------------------------------------------------------------------- + + * 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 Unit tests for unit quaternions + * @author Frank Dellaert + **/ + +#include + +namespace gtsam { + +/// Typedef to an Eigen Quaternion, 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 Quaternion; + +namespace traits { +/// Define Quaternion to be a model of the Group concept +template<> +struct structure_category { + typedef group_tag type; +}; +} // \namespace gtsam::traits + +namespace group { + +Quaternion compose(const Quaternion&g, const Quaternion& h) { + return g * h; +} + +Quaternion between(const Quaternion&g, const Quaternion& h) { + return g.inverse() * h; +} + +Quaternion inverse(const Quaternion&g) { + return g.inverse(); +} + +namespace traits { + +/// Define the trait that specifies Quaternion's identity element +template<> +struct identity { + static const Quaternion value; + typedef Quaternion value_type; +}; + +const Quaternion identity::value = Quaternion(0); + +/// Define the trait that asserts Quaternion is an additive group +template<> +struct flavor { + typedef multiplicative_tag type; +}; + +} // \namespace gtsam::group::traits +} // \namespace gtsam::group +} // \namespace gtsam + +/** + * @file testCyclic.cpp + * @brief Unit tests for cyclic group + * @author Frank Dellaert + **/ + +//#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef Quaternion Q; // Typedef + +//****************************************************************************** +TEST(Quaternion, Concept) { + BOOST_CONCEPT_ASSERT((Group)); +} + +//****************************************************************************** +TEST(Quaternion, Constructor) { + Q g(0); +} + +//****************************************************************************** +TEST(Quaternion, Compose) { +} + +//****************************************************************************** +TEST(Quaternion, Between) { +} + +//****************************************************************************** +TEST(Quaternion, Ivnverse) { +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + From 06640cc4144f5c675b84f473e66f21e4a72c5158 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 09:48:31 +0100 Subject: [PATCH 043/199] New naming convention for concept (IsGroup), moved invariant checking out to namespace --- gtsam/base/concepts.h | 77 ++++++++++++------------- gtsam/geometry/tests/testCyclic.cpp | 2 +- gtsam/geometry/tests/testQuaternion.cpp | 7 ++- 3 files changed, 43 insertions(+), 43 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index dc4fca036..a6b2448c2 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -11,6 +11,7 @@ //#include "manifold.h" //#include "chart.h" #include +#include #include #include #include @@ -24,7 +25,7 @@ namespace traits { * @brief Associate a unique tag with each of the main GTSAM concepts */ //@{ -template +template struct structure_category; // specializations should be derived from one of the following tags //@} @@ -50,14 +51,14 @@ namespace traits { /** @name Manifold Traits */ //@{ -template struct TangentVector; -template struct DefaultChart; +template struct TangentVector; +template struct DefaultChart; //@} }// namespace traits /* - template + template class ManifoldConcept { public: typedef T Manifold; @@ -75,7 +76,7 @@ template struct DefaultChart; TangentVector v; }; - template + template class ChartConcept { public: typedef C Chart; @@ -107,8 +108,8 @@ namespace traits { /** @name Group Traits */ //@{ -template struct identity; -template struct flavor; +template struct identity; +template struct flavor; //@} /** @name Group Flavor Tags */ @@ -120,62 +121,60 @@ struct multiplicative_tag { //@} } // \ namespace traits + +/// Check invariants +template +//BOOST_CONCEPT_REQUIRES((Testable)) +bool check_invariants(const G& a, const G& b) { + G e = traits::identity::value; + typename traits::flavor::type flavor; + return (equal(compose(a, inverse(a)), e)) + && (equal(between(a, b), compose(inverse(a), b))) + && (equal(compose(a, between(a, b)), b)) // + && operator_usage(a, b, flavor); +} } // \ namespace group /** * Group Concept */ template -class Group { +class IsGroup { public: typedef typename traits::structure_category::type structure_category_tag; typedef typename group::traits::identity::value_type identity_value_type; typedef typename group::traits::flavor::type flavor_tag; - BOOST_CONCEPT_USAGE(Group) { + void operator_usage(group::traits::multiplicative_tag) { + g = g * h; + } + void operator_usage(group::traits::additive_tag) { + g = g + h; + g = h - g; + g = -g; + } + + BOOST_CONCEPT_USAGE(IsGroup) { using group::compose; using group::between; using group::inverse; BOOST_STATIC_ASSERT( boost::is_base_of::value); e = group::traits::identity::value; - d = compose(g, h); - d = between(g, h); - ig = inverse(g); - test = operator_usage(g, h, flavor); + g = compose(g, h); + g = between(g, h); + g = inverse(g); + operator_usage(flavor); } -// TODO: these all require default constructors :-( -// Also, requires equal which is not required of a group -// Group():e(group::traits::identity::value) { -// } -// -// bool check_invariants(const G& a, const G& b) { -// return (equal(compose(a, inverse(a)), e)) -// && (equal(between(a, b), compose(inverse(a), b))) -// && (equal(compose(a, between(a, b)), b)) -// && operator_usage(a, b, flavor); -// } - private: flavor_tag flavor; - G e, g, h, gh, ig, d; - bool test, test2; - - bool operator_usage(const G& a, const G& b, - group::traits::multiplicative_tag) { -// return group::compose(a, b) == a * b; - return true; - } - bool operator_usage(const G& a, const G& b, group::traits::additive_tag) { - return group::compose(a, b) == a + b; - } - + G e, g, h; }; /* - template + template class LieGroupConcept : public GroupConcept, public ManifoldConcept { BOOST_CONCEPT_USAGE(LieGroupConcept) { @@ -183,7 +182,7 @@ private: } }; - template + template class VectorSpaceConcept : public LieGroupConcept { typedef typename traits::DefaultChart::type Chart; typedef typename GroupConcept::identity identity; diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 00ea2c853..f9d4a2d77 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -26,7 +26,7 @@ typedef Cyclic<6> G; // Let's use the cyclic group of order 6 //****************************************************************************** TEST(Cyclic, Concept) { - BOOST_CONCEPT_ASSERT((Group)); + BOOST_CONCEPT_ASSERT((IsGroup)); EXPECT_LONGS_EQUAL(0, group::traits::identity::value); G g(2), h(3); // EXPECT(Group().check_invariants(g,h)) diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index a35576259..51ea99d14 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -55,7 +55,7 @@ struct identity { typedef Quaternion value_type; }; -const Quaternion identity::value = Quaternion(0); +const Quaternion identity::value = Quaternion::Identity(); /// Define the trait that asserts Quaternion is an additive group template<> @@ -75,6 +75,7 @@ struct flavor { //#include #include +#include #include using namespace std; @@ -84,12 +85,12 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion, Concept) { - BOOST_CONCEPT_ASSERT((Group)); + BOOST_CONCEPT_ASSERT((IsGroup)); } //****************************************************************************** TEST(Quaternion, Constructor) { - Q g(0); + Q g(Eigen::AngleAxisd(1, Vector3(0,0,1))); } //****************************************************************************** From 3a6b89e840abae4126ac20863406f3895537eca2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 10:19:38 +0100 Subject: [PATCH 044/199] Now all Eigen::Quaternion flavors are certified as IsGroup --- gtsam/geometry/tests/testQuaternion.cpp | 68 +++++++++++++++---------- 1 file changed, 41 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 51ea99d14..90aeffe28 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -19,47 +19,54 @@ namespace gtsam { -/// Typedef to an Eigen Quaternion, 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 Quaternion; - namespace traits { -/// Define Quaternion to be a model of the Group concept -template<> -struct structure_category { +/// Define Eigen::Quaternion to be a model of the Group concept +template +struct structure_category > { typedef group_tag type; }; } // \namespace gtsam::traits namespace group { -Quaternion compose(const Quaternion&g, const Quaternion& h) { +template +Eigen::Quaternion<_Scalar, _Options> compose( + const Eigen::Quaternion<_Scalar, _Options> &g, + const Eigen::Quaternion<_Scalar, _Options> & h) { return g * h; } -Quaternion between(const Quaternion&g, const Quaternion& h) { +template +Eigen::Quaternion<_Scalar, _Options> between( + const Eigen::Quaternion<_Scalar, _Options> &g, + const Eigen::Quaternion<_Scalar, _Options> & h) { return g.inverse() * h; } -Quaternion inverse(const Quaternion&g) { +template +Eigen::Quaternion<_Scalar, _Options> inverse( + const Eigen::Quaternion<_Scalar, _Options> &g) { return g.inverse(); } namespace traits { -/// Define the trait that specifies Quaternion's identity element -template<> -struct identity { - static const Quaternion value; - typedef Quaternion value_type; +/// Declare the trait that specifies a quaternion's identity element +template +struct identity > { + static const Eigen::Quaternion<_Scalar, _Options> value; + typedef Eigen::Quaternion<_Scalar, _Options> value_type; }; -const Quaternion identity::value = Quaternion::Identity(); +/// Out of line definition of identity +template +const Eigen::Quaternion<_Scalar, _Options> identity< + Eigen::Quaternion<_Scalar, _Options> >::value = Eigen::Quaternion<_Scalar, + _Options>::Identity(); -/// Define the trait that asserts Quaternion is an additive group -template<> -struct flavor { +/// Define the trait that asserts quaternions are a multiplicative group +template +struct flavor > { typedef multiplicative_tag type; }; @@ -67,6 +74,13 @@ struct flavor { } // \namespace gtsam::group } // \namespace gtsam +/** + * GSAM typedef to an Eigen::Quaternion, 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 Quaternion; + /** * @file testCyclic.cpp * @brief Unit tests for cyclic group @@ -84,25 +98,25 @@ using namespace gtsam; typedef Quaternion Q; // Typedef //****************************************************************************** -TEST(Quaternion, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); +TEST(Quaternion , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); } //****************************************************************************** -TEST(Quaternion, Constructor) { - Q g(Eigen::AngleAxisd(1, Vector3(0,0,1))); +TEST(Quaternion , Constructor) { + Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** -TEST(Quaternion, Compose) { +TEST(Quaternion , Compose) { } //****************************************************************************** -TEST(Quaternion, Between) { +TEST(Quaternion , Between) { } //****************************************************************************** -TEST(Quaternion, Ivnverse) { +TEST(Quaternion , Ivnverse) { } //****************************************************************************** From 187760ce95ad28b64ff774c06e49c90b195af1dd Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 10:20:54 +0100 Subject: [PATCH 045/199] Refactor for readability --- gtsam/geometry/tests/testQuaternion.cpp | 42 +++++++++++-------------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 90aeffe28..339897a06 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -21,52 +21,48 @@ namespace gtsam { namespace traits { /// Define Eigen::Quaternion to be a model of the Group concept -template -struct structure_category > { +template +struct structure_category > { typedef group_tag type; }; } // \namespace gtsam::traits namespace group { -template -Eigen::Quaternion<_Scalar, _Options> compose( - const Eigen::Quaternion<_Scalar, _Options> &g, - const Eigen::Quaternion<_Scalar, _Options> & h) { +template +Eigen::Quaternion compose(const Eigen::Quaternion &g, + const Eigen::Quaternion & h) { return g * h; } -template -Eigen::Quaternion<_Scalar, _Options> between( - const Eigen::Quaternion<_Scalar, _Options> &g, - const Eigen::Quaternion<_Scalar, _Options> & h) { +template +Eigen::Quaternion between(const Eigen::Quaternion &g, + const Eigen::Quaternion & h) { return g.inverse() * h; } -template -Eigen::Quaternion<_Scalar, _Options> inverse( - const Eigen::Quaternion<_Scalar, _Options> &g) { +template +Eigen::Quaternion inverse(const Eigen::Quaternion &g) { return g.inverse(); } namespace traits { /// Declare the trait that specifies a quaternion's identity element -template -struct identity > { - static const Eigen::Quaternion<_Scalar, _Options> value; - typedef Eigen::Quaternion<_Scalar, _Options> value_type; +template +struct identity > { + static const Eigen::Quaternion value; + typedef Eigen::Quaternion value_type; }; /// Out of line definition of identity -template -const Eigen::Quaternion<_Scalar, _Options> identity< - Eigen::Quaternion<_Scalar, _Options> >::value = Eigen::Quaternion<_Scalar, - _Options>::Identity(); +template +const Eigen::Quaternion identity >::value = + Eigen::Quaternion::Identity(); /// Define the trait that asserts quaternions are a multiplicative group -template -struct flavor > { +template +struct flavor > { typedef multiplicative_tag type; }; From 01aab775041cb68427487d538fe71731f1e50cf5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 11:51:40 +0100 Subject: [PATCH 046/199] Made Testable a boost concept --- gtsam/base/Testable.h | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index a308c50a1..83c2fa4e8 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -50,17 +51,20 @@ namespace gtsam { * @tparam T is the type this constrains to be testable - assumes print() and equals() */ template - class TestableConcept { - static bool checkTestableConcept(const T& d) { + class Testable { + T t; + bool r1,r2; + public: + + BOOST_CONCEPT_USAGE(Testable) { // check print function, with optional string - d.print(std::string()); - d.print(); + t.print(std::string()); + t.print(); // check print, with optional threshold double tol = 1.0; - bool r1 = d.equals(d, tol); - bool r2 = d.equals(d); - return r1 && r2; + r1 = t.equals(t, tol); + r2 = t.equals(t); } }; @@ -129,6 +133,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; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept _gtsam_TestableConcept_##T; +#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::Testable; +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::Testable _gtsam_Testable_##T; From e2f250c160b7e2241f471ae5615ab1cdbdf872d6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 11:52:09 +0100 Subject: [PATCH 047/199] Added Manifold, Lie Group, and Vector Space concepts back in --- gtsam/base/concepts.h | 240 ++++++++++++++---------- gtsam/geometry/tests/testCyclic.cpp | 6 + gtsam/geometry/tests/testQuaternion.cpp | 49 ++++- 3 files changed, 195 insertions(+), 100 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index a6b2448c2..6caf1703f 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -25,9 +26,7 @@ namespace traits { * @brief Associate a unique tag with each of the main GTSAM concepts */ //@{ -template -struct structure_category; -// specializations should be derived from one of the following tags +template struct structure_category; //@} /** @@ -35,81 +34,95 @@ struct structure_category; * @brief Possible values for traits::structure_category::type */ //@{ -struct manifold_tag { -}; -struct group_tag { -}; -struct lie_group_tag: public manifold_tag, public group_tag { -}; -struct vector_space_tag: public lie_group_tag { -}; +struct manifold_tag {}; +struct group_tag {}; +struct lie_group_tag: public manifold_tag, public group_tag {}; +struct vector_space_tag: public lie_group_tag {}; //@} }// namespace traits +namespace manifold { + +/** @name Free functions any Manifold needs to define */ +//@{ +//@} + namespace traits { /** @name Manifold Traits */ //@{ +template struct dimension; template struct TangentVector; template struct DefaultChart; //@} -}// namespace traits +}// \ namespace traits -/* - template - class ManifoldConcept { - public: - typedef T Manifold; - typedef typename traits::TangentVector::type TangentVector; - typedef typename traits::DefaultChart::type DefaultChart; - static const size_t dim = traits::dimension::value; +/// Check invariants for Manifold type +template +BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) // +check_invariants(const T& a, const T& b) { + typedef typename traits::DefaultChart::type Chart; + return true; +} - BOOST_CONCEPT_USAGE(ManifoldConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - // no direct usage for manifold since most usage is through a chart - } - private: - Manifold p; - TangentVector v; - }; - - template - class ChartConcept { - public: - typedef C Chart; - typedef typename traits::Manifold::type Manifold; - typedef typename traits::TangentVector::type TangentVector; - - BOOST_CONCEPT_USAGE(ChartConcept) { - v = Chart::local(p,q); // returns local coordinates of q w.r.t. origin p - q = Chart::retract(p,v); // returns retracted update of p with v - } - - private: - Manifold p,q; - TangentVector v; - - }; +/** + * Base class for Charts + * Derived has to implement local and retract as static methods */ +template +struct Chart { + typedef T ManifoldType; + typedef typename traits::TangentVector::type TangentVector; + static TangentVector Local(const ManifoldType& p, const ManifoldType& q) { + return Derived::local(p, q); + } + static ManifoldType Retract(const ManifoldType& p, const TangentVector& v) { + return Derived::retract(p, v); + } +protected: + Chart() { + (void) &Local; + (void) &Retract; + } // enforce early instantiation. +}; + +} // \ namespace manifold + +template +class IsManifold { +public: + typedef typename traits::structure_category::type structure_category_tag; + static const size_t dim = manifold::traits::dimension::value; + typedef typename manifold::traits::TangentVector::type TangentVector; + typedef typename manifold::traits::DefaultChart::type DefaultChart; + + BOOST_CONCEPT_USAGE(IsManifold) { + BOOST_STATIC_ASSERT(boost::is_base_of::value, "This type's structure_category trait does not assert it as a manifold (or derived)"); + BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); + // no direct usage for manifold since most usage is through a chart + } +private: + T p; + TangentVector v; +}; namespace group { /** @name Free functions any Group needs to define */ //@{ -template G compose(const G&g, const G& h); -template G between(const G&g, const G& h); -template G inverse(const G&g); +template T compose(const T&g, const T& h); +template T between(const T&g, const T& h); +template T inverse(const T&g); //@} namespace traits { /** @name Group Traits */ //@{ -template struct identity; -template struct flavor; +template struct identity; +template struct flavor; //@} /** @name Group Flavor Tags */ @@ -120,31 +133,29 @@ struct multiplicative_tag { }; //@} -} // \ namespace traits +}// \ namespace traits /// Check invariants -template -//BOOST_CONCEPT_REQUIRES((Testable)) -bool check_invariants(const G& a, const G& b) { - G e = traits::identity::value; - typename traits::flavor::type flavor; - return (equal(compose(a, inverse(a)), e)) - && (equal(between(a, b), compose(inverse(a), b))) - && (equal(compose(a, between(a, b)), b)) // - && operator_usage(a, b, flavor); +template +BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) // +check_invariants(const T& a, const T& b, double tol = 1e-9) { + T e = traits::identity::value; + return compose(a, inverse(a)).equals(e, tol) + && between(a, b).equals(compose(inverse(a), b), tol) + && compose(a, between(a, b)).equals(b, tol); } } // \ namespace group /** * Group Concept */ -template +template class IsGroup { public: - typedef typename traits::structure_category::type structure_category_tag; - typedef typename group::traits::identity::value_type identity_value_type; - typedef typename group::traits::flavor::type flavor_tag; + typedef typename traits::structure_category::type structure_category_tag; + typedef typename group::traits::identity::value_type identity_value_type; + typedef typename group::traits::flavor::type flavor_tag; void operator_usage(group::traits::multiplicative_tag) { g = g * h; @@ -159,9 +170,8 @@ public: using group::compose; using group::between; using group::inverse; - BOOST_STATIC_ASSERT( - boost::is_base_of::value); - e = group::traits::identity::value; + BOOST_STATIC_ASSERT( boost::is_base_of::value, "This type's structure_category trait does not assert it as a group (or derived)"); + e = group::traits::identity::value; g = compose(g, h); g = between(g, h); g = inverse(g); @@ -170,42 +180,78 @@ public: private: flavor_tag flavor; - G e, g, h; + T e, g, h; }; -/* - template - class LieGroupConcept : public GroupConcept, public ManifoldConcept { +namespace lie_group { - BOOST_CONCEPT_USAGE(LieGroupConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - } - }; +/** @name Free functions any Group needs to define */ +//@{ +// TODO need Jacobians +//template T compose(const T&g, const T& h); +//template T between(const T&g, const T& h); +//template T inverse(const T&g); +//@} - template - class VectorSpaceConcept : public LieGroupConcept { - typedef typename traits::DefaultChart::type Chart; - typedef typename GroupConcept::identity identity; +namespace traits { - BOOST_CONCEPT_USAGE(VectorSpaceConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - r = p+q; - r = -p; - r = p-q; - } +/** @name Lie Group Traits */ +//@{ +//@} - bool check_invariants(const V& a, const V& b) { - return equal(compose(a, b), a+b) - && equal(inverse(a), -a) - && equal(between(a, b), b-a) - && equal(Chart::retract(a, b), a+b) - && equal(Chart::local(a, b), b-a); - } +}// \ namespace traits - private: - V g,q,r; - }; +/// Check invariants +//template +//BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) check_invariants(const T& a, +// const T& b) { +// bool check_invariants(const V& a, const V& b) { +// return equal(Chart::retract(a, b), a + b) +// && equal(Chart::local(a, b), b - a); +// } +//} +}// \ namespace lie_group + +/** + * Lie Group Concept */ +template +class IsLieGroup: public IsGroup, public IsManifold { +public: + + typedef typename traits::structure_category::type structure_category_tag; + + BOOST_CONCEPT_USAGE(IsLieGroup) { + BOOST_STATIC_ASSERT(boost::is_base_of::value,"This type's trait does not assert it as a Lie group (or derived)"); + // TODO Check with Jacobian +// using lie_group::compose; +// using lie_group::between; +// using lie_group::inverse; +// g = compose(g, h); +// g = between(g, h); +// g = inverse(g); + } +private: + + T g, h; +}; + +template +class IsVectorSpace: public IsLieGroup { +public: + + typedef typename traits::structure_category::type structure_category_tag; + + BOOST_CONCEPT_USAGE(IsVectorSpace) { + BOOST_STATIC_ASSERT(boost::is_base_of::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 diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index f9d4a2d77..3cfa6b2c8 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -81,6 +81,12 @@ TEST(Cyclic, Ivnverse) { EXPECT_LONGS_EQUAL(1, group::inverse(G(5))); } +//****************************************************************************** +TEST(Cyclic , Invariants) { + G g(2), h(5); + group::check_invariants(g,h); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 339897a06..22a96f4a3 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -20,13 +20,47 @@ namespace gtsam { namespace traits { -/// Define Eigen::Quaternion to be a model of the Group concept + +/// Define Eigen::Quaternion to be a model of the Lie Group concept template struct structure_category > { - typedef group_tag type; + typedef lie_group_tag type; }; + } // \namespace gtsam::traits +namespace manifold { + +/// Chart for Eigen Quaternions +template +class QuaternionChart: public manifold::Chart, + QuaternionChart > { + +}; + +namespace traits { + +/// Define the trait that asserts Quaternion manifold has dimension 3 +template +struct dimension > : public boost::integral_constant< + int, 3> { +}; + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct TangentVector > { + typedef Eigen::Matrix type; +}; + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct DefaultChart > { + typedef QuaternionChart type; +}; + +} // \namespace gtsam::manifold::traits +} // \namespace gtsam::manifold + namespace group { template @@ -95,7 +129,9 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); + // BOOST_CONCEPT_ASSERT((IsGroup)); + // BOOST_CONCEPT_ASSERT((IsManifold)); + // BOOST_CONCEPT_ASSERT((IsLieGroup)); } //****************************************************************************** @@ -103,6 +139,13 @@ TEST(Quaternion , Constructor) { Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } +//****************************************************************************** +TEST(Quaternion , Invariants) { + Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q h(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + // group::check_invariants(g,h); Does not satisfy Testable concept (yet!) +} + //****************************************************************************** TEST(Quaternion , Compose) { } From 8db8cb54b0ed426e0c98cbc406336b667c35f44a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 12:35:05 +0100 Subject: [PATCH 048/199] Some chart refinement (early check does not work) --- gtsam/base/concepts.h | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 6caf1703f..2c2a4e07c 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -85,7 +85,7 @@ protected: Chart() { (void) &Local; (void) &Retract; - } // enforce early instantiation. + } // enforce early instantiation. TODO does not seem to work }; } // \ namespace manifold @@ -99,12 +99,19 @@ public: typedef typename manifold::traits::DefaultChart::type DefaultChart; BOOST_CONCEPT_USAGE(IsManifold) { - BOOST_STATIC_ASSERT(boost::is_base_of::value, "This type's structure_category trait does not assert it as a manifold (or derived)"); + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a manifold (or derived)"); BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - // no direct usage for manifold since most usage is through a chart + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of, DefaultChart>::value), + "This type's DefaultChart does not derive from manifold::Chart, as required"); + // make sure Derived methods in Chart are defined + v = DefaultChart::local(p,q); + q = DefaultChart::retract(p,v); } private: - T p; + T p,q; TangentVector v; }; @@ -170,7 +177,9 @@ public: using group::compose; using group::between; using group::inverse; - BOOST_STATIC_ASSERT( boost::is_base_of::value, "This type's structure_category trait does not assert it as a group (or derived)"); + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a group (or derived)"); e = group::traits::identity::value; g = compose(g, h); g = between(g, h); @@ -222,7 +231,9 @@ public: typedef typename traits::structure_category::type structure_category_tag; BOOST_CONCEPT_USAGE(IsLieGroup) { - BOOST_STATIC_ASSERT(boost::is_base_of::value,"This type's trait does not assert it as a Lie group (or derived)"); + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it as a Lie group (or derived)"); // TODO Check with Jacobian // using lie_group::compose; // using lie_group::between; @@ -243,7 +254,9 @@ public: typedef typename traits::structure_category::type structure_category_tag; BOOST_CONCEPT_USAGE(IsVectorSpace) { - BOOST_STATIC_ASSERT(boost::is_base_of::value,"This type's trait does not assert it as a vector space (or derived)"); + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it as a vector space (or derived)"); r = p + q; r = -p; r = p - q; From 36da8702f96ddcb26ef626ee1b2514dda8e451c9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 12:35:17 +0100 Subject: [PATCH 049/199] Retract works --- gtsam/geometry/tests/testQuaternion.cpp | 58 ++++++++++++++++++++----- 1 file changed, 46 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 22a96f4a3..8c8f9354f 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -33,9 +33,19 @@ namespace manifold { /// Chart for Eigen Quaternions template -class QuaternionChart: public manifold::Chart, +struct QuaternionChart: public manifold::Chart, QuaternionChart > { - + typedef Eigen::Quaternion Q; + typedef typename traits::TangentVector::type V; + static V local(const Q& p, const Q& q) { + return V(); + } + static Q retract(const Q& p, const V& v) { + double theta = v.norm(); + if (std::abs(theta) < 1e-10) + return p; + return p * Q(Eigen::AngleAxisd(theta, v / theta)); + } }; namespace traits { @@ -49,13 +59,13 @@ struct dimension > : public boost::integral_constant< /// Define the trait that asserts Quaternion TangentVector is Vector3 template struct TangentVector > { - typedef Eigen::Matrix type; + typedef Eigen::Matrix type; }; /// Define the trait that asserts Quaternion TangentVector is Vector3 template struct DefaultChart > { - typedef QuaternionChart type; + typedef QuaternionChart type; }; } // \namespace gtsam::manifold::traits @@ -129,21 +139,45 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion , Concept) { - // BOOST_CONCEPT_ASSERT((IsGroup)); - // BOOST_CONCEPT_ASSERT((IsManifold)); - // BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); // not strictly needed + BOOST_CONCEPT_ASSERT((IsManifold)); // not strictly needed + BOOST_CONCEPT_ASSERT((IsLieGroup)); } //****************************************************************************** TEST(Quaternion , Constructor) { - Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** TEST(Quaternion , Invariants) { - Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); - Q h(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); - // group::check_invariants(g,h); Does not satisfy Testable concept (yet!) + Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + // group::check_invariants(q1,q2); Does not satisfy Testable concept (yet!) +} + +//****************************************************************************** +TEST(Quaternion , Local) { + Vector3 z_axis(0,0,1); + Q q1(Eigen::AngleAxisd(0,z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + typedef manifold::traits::DefaultChart::type Chart; + Vector3 expected(0,0,0.1); + Vector3 actual = Chart::Local(q1,q2); + cout << expected << endl; + cout << actual << endl; + EXPECT(assert_equal((Vector)expected,actual)); +} + +//****************************************************************************** +TEST(Quaternion , Retract) { + Vector3 z_axis(0,0,1); + Q q(Eigen::AngleAxisd(0,z_axis)); + Q expected(Eigen::AngleAxisd(0.1, z_axis)); + typedef manifold::traits::DefaultChart::type Chart; + Vector3 v(0,0,0.1); + Q actual = Chart::Retract(q,v); + EXPECT(actual.isApprox(expected)); } //****************************************************************************** @@ -155,7 +189,7 @@ TEST(Quaternion , Between) { } //****************************************************************************** -TEST(Quaternion , Ivnverse) { +TEST(Quaternion , Inverse) { } //****************************************************************************** From a31e596448aec80505c6bb6fc4b80c27f0f44f13 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 12:47:26 +0100 Subject: [PATCH 050/199] Working local/Logmap (taken from Rot3Q) --- gtsam/base/concepts.h | 3 ++ gtsam/geometry/tests/testQuaternion.cpp | 72 +++++++++++++++++++------ 2 files changed, 58 insertions(+), 17 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 2c2a4e07c..0af22171e 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -75,6 +75,9 @@ template struct Chart { typedef T ManifoldType; typedef typename traits::TangentVector::type TangentVector; + + // TODO, maybe we need Retract and Local to be unary, or both + // TOOD, also, this indirection mechanism does not seem to help static TangentVector Local(const ManifoldType& p, const ManifoldType& q) { return Derived::local(p, q); } diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 8c8f9354f..ef4305948 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -37,14 +37,50 @@ struct QuaternionChart: public manifold::Chart, QuaternionChart > { typedef Eigen::Quaternion Q; typedef typename traits::TangentVector::type V; - static V local(const Q& p, const Q& q) { - return V(); - } - static Q retract(const Q& p, const V& v) { - double theta = v.norm(); + + /// Exponential map, simply be converting omega to AngleAxis + static Q Expmap(const V& omega) { + double theta = omega.norm(); if (std::abs(theta) < 1e-10) - return p; - return p * Q(Eigen::AngleAxisd(theta, v / theta)); + return Q::Identity(); + return Q(Eigen::AngleAxisd(theta, omega / theta)); + } + + /// retract, simply be converting omega to AngleAxis + static Q retract(const Q& p, const V& omega) { + return p * Expmap(omega); + } + + /// We use our own Logmap, as there is a slight bug in Eigen + static V Logmap(const Q& q) { + 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 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(); + } + } + + /// local is our own, as there is a slight bug in Eigen + static V local(const Q& q1, const Q& q2) { + return Logmap(q1.inverse() * q2); } }; @@ -139,8 +175,10 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); // not strictly needed - BOOST_CONCEPT_ASSERT((IsManifold)); // not strictly needed + BOOST_CONCEPT_ASSERT((IsGroup)); + // not strictly needed + BOOST_CONCEPT_ASSERT((IsManifold)); + // not strictly needed BOOST_CONCEPT_ASSERT((IsLieGroup)); } @@ -158,12 +196,12 @@ TEST(Quaternion , Invariants) { //****************************************************************************** TEST(Quaternion , Local) { - Vector3 z_axis(0,0,1); - Q q1(Eigen::AngleAxisd(0,z_axis)); + Vector3 z_axis(0, 0, 1); + Q q1(Eigen::AngleAxisd(0, z_axis)); Q q2(Eigen::AngleAxisd(0.1, z_axis)); typedef manifold::traits::DefaultChart::type Chart; - Vector3 expected(0,0,0.1); - Vector3 actual = Chart::Local(q1,q2); + Vector3 expected(0, 0, 0.1); + Vector3 actual = Chart::Local(q1, q2); cout << expected << endl; cout << actual << endl; EXPECT(assert_equal((Vector)expected,actual)); @@ -171,12 +209,12 @@ TEST(Quaternion , Local) { //****************************************************************************** TEST(Quaternion , Retract) { - Vector3 z_axis(0,0,1); - Q q(Eigen::AngleAxisd(0,z_axis)); + Vector3 z_axis(0, 0, 1); + Q q(Eigen::AngleAxisd(0, z_axis)); Q expected(Eigen::AngleAxisd(0.1, z_axis)); typedef manifold::traits::DefaultChart::type Chart; - Vector3 v(0,0,0.1); - Q actual = Chart::Retract(q,v); + Vector3 v(0, 0, 0.1); + Q actual = Chart::Retract(q, v); EXPECT(actual.isApprox(expected)); } From cdc0029158df2a7a2631fb5fd98a55298763356c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 13:01:25 +0100 Subject: [PATCH 051/199] Reverted on Chart base class. But no chart-specific traits needed, as assumed created by us. --- GTSAM-Concepts.md | 24 ----------- gtsam/base/concepts.h | 53 +++++++++++-------------- gtsam/geometry/tests/testQuaternion.cpp | 24 +++++------ 3 files changed, 35 insertions(+), 66 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 77c1621bf..1e7960b56 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -146,30 +146,6 @@ TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts. and we also define a limited number of `gtsam::tags` to select the correct implementation of certain functions at compile time (tag dispatching). Charts are done more conventionally, so we start there... -Interfaces ----------- - -Because Charts are always written by the user (or automatically generated, see below for vector spaces), -we enforce the Chart concept using an abstract base class, acting as an interface: - -``` -#!c++ -template -struct Chart { - typedef T ManifoldType; - typedef typename traits::TangentVector::type TangentVector; - static TangentVector Local(const ManifoldType& p, const ManifoldType& q) {return Derived::local(p,q);} - static ManifoldType Retract(const ManifoldType& p, const TangentVector& v) {return Derived::retract(p,v);} - protected: - Chart(){ (void)&Local; (void)&Retract; } // enforce early instantiation. -} -``` - -The [CRTP](http://en.wikipedia.org/wiki/Curiously_recurring_template_pattern) and the protected constructor -automatically check for the existence of the methods in the Derived class, whenever a new Chart is created by - - struct MyChart : Chart { ... } - Traits ------ diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 0af22171e..3f1222cec 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -67,32 +67,30 @@ check_invariants(const T& a, const T& b) { return true; } -/** - * Base class for Charts - * Derived has to implement local and retract as static methods - */ -template -struct Chart { - typedef T ManifoldType; - typedef typename traits::TangentVector::type TangentVector; - - // TODO, maybe we need Retract and Local to be unary, or both - // TOOD, also, this indirection mechanism does not seem to help - static TangentVector Local(const ManifoldType& p, const ManifoldType& q) { - return Derived::local(p, q); - } - static ManifoldType Retract(const ManifoldType& p, const TangentVector& v) { - return Derived::retract(p, v); - } -protected: - Chart() { - (void) &Local; - (void) &Retract; - } // enforce early instantiation. TODO does not seem to work -}; - } // \ namespace manifold +/** + * Chart concept + */ +template +class IsChart { +public: + typedef typename T::ManifoldType ManifoldType; + typedef typename manifold::traits::TangentVector::type V; + + BOOST_CONCEPT_USAGE(IsChart) { + // make sure Derived methods in Chart are defined + v = T::Local(p,q); + q = T::Retract(p,v); + } +private: + ManifoldType p,q; + V v; +}; + +/** + * Manifold concept + */ template class IsManifold { public: @@ -106,12 +104,7 @@ public: (boost::is_base_of::value), "This type's structure_category trait does not assert it as a manifold (or derived)"); BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of, DefaultChart>::value), - "This type's DefaultChart does not derive from manifold::Chart, as required"); - // make sure Derived methods in Chart are defined - v = DefaultChart::local(p,q); - q = DefaultChart::retract(p,v); + BOOST_CONCEPT_ASSERT((IsChart)); } private: T p,q; diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index ef4305948..89dcd3024 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -33,13 +33,17 @@ namespace manifold { /// Chart for Eigen Quaternions template -struct QuaternionChart: public manifold::Chart, - QuaternionChart > { - typedef Eigen::Quaternion Q; - typedef typename traits::TangentVector::type V; +struct QuaternionChart { + + // required + typedef Eigen::Quaternion ManifoldType; + + // internal + typedef ManifoldType Q; + typedef typename traits::TangentVector::type Omega; /// Exponential map, simply be converting omega to AngleAxis - static Q Expmap(const V& omega) { + static Q Expmap(const Omega& omega) { double theta = omega.norm(); if (std::abs(theta) < 1e-10) return Q::Identity(); @@ -47,12 +51,12 @@ struct QuaternionChart: public manifold::Chart, } /// retract, simply be converting omega to AngleAxis - static Q retract(const Q& p, const V& omega) { + static Q Retract(const Q& p, const Omega& omega) { return p * Expmap(omega); } /// We use our own Logmap, as there is a slight bug in Eigen - static V Logmap(const Q& q) { + static Omega Logmap(const Q& q) { using std::acos; using std::sqrt; static const double twoPi = 2.0 * M_PI, @@ -79,7 +83,7 @@ struct QuaternionChart: public manifold::Chart, } /// local is our own, as there is a slight bug in Eigen - static V local(const Q& q1, const Q& q2) { + static Omega Local(const Q& q1, const Q& q2) { return Logmap(q1.inverse() * q2); } }; @@ -176,9 +180,7 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); - // not strictly needed BOOST_CONCEPT_ASSERT((IsManifold)); - // not strictly needed BOOST_CONCEPT_ASSERT((IsLieGroup)); } @@ -202,8 +204,6 @@ TEST(Quaternion , Local) { typedef manifold::traits::DefaultChart::type Chart; Vector3 expected(0, 0, 0.1); Vector3 actual = Chart::Local(q1, q2); - cout << expected << endl; - cout << actual << endl; EXPECT(assert_equal((Vector)expected,actual)); } From 022e9300856e0d082b9d9d90b9d447fb08fbdc6e Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 13:23:59 +0100 Subject: [PATCH 052/199] Fixed typo --- gtsam/base/concepts.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 3f1222cec..0ffb2fbd2 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -145,7 +145,7 @@ check_invariants(const T& a, const T& b, double tol = 1e-9) { T e = traits::identity::value; return compose(a, inverse(a)).equals(e, tol) && between(a, b).equals(compose(inverse(a), b), tol) - && compose(a, between(a, b)).equals(b, tol); + && compose(a, between(a, b)).equals(b, tol); } } // \ namespace group From ef58a8a56ab21a61afacb70e9a4ad005e7e65da9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 13:24:59 +0100 Subject: [PATCH 053/199] Attempt at satisfying Group concept by deriving from base class. Needs to be fixed, also, test does not link :-( --- gtsam/geometry/Cyclic.h | 35 +++++++++++++++++++++-------- gtsam/geometry/tests/testCyclic.cpp | 5 +++-- 2 files changed, 29 insertions(+), 11 deletions(-) diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 71c7cbc4a..aded063e7 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -20,8 +20,15 @@ namespace gtsam { +/// Additive Group +template +class AdditiveGroup { + +}; + +/// Cyclic group of order N template -class Cyclic { +class Cyclic : AdditiveGroup > { size_t i_; ///< we just use an unsigned int public: /// Constructor @@ -44,6 +51,16 @@ public: 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_; + } + }; namespace traits { @@ -55,18 +72,18 @@ template struct structure_category > { namespace group { -template -Cyclic compose(const Cyclic&g, const Cyclic& h) { +template +AdditiveGroup compose(const AdditiveGroup&g, const AdditiveGroup& h) { return g + h; } -template -Cyclic between(const Cyclic&g, const Cyclic& h) { +template +AdditiveGroup between(const AdditiveGroup&g, const AdditiveGroup& h) { return h - g; } -template -Cyclic inverse(const Cyclic&g) { +template +AdditiveGroup inverse(const AdditiveGroup&g) { return -g; } @@ -81,8 +98,8 @@ template struct identity > { template const Cyclic identity >::value = Cyclic(0); -/// Define the trait that asserts Cyclic is an additive group -template struct flavor > { +/// Define the trait that asserts AdditiveGroup is an additive group +template struct flavor > { typedef additive_tag type; }; diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 3cfa6b2c8..1a30beb1c 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -26,7 +26,8 @@ typedef Cyclic<6> G; // Let's use the cyclic group of order 6 //****************************************************************************** TEST(Cyclic, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); +// BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((AdditiveGroup)); EXPECT_LONGS_EQUAL(0, group::traits::identity::value); G g(2), h(3); // EXPECT(Group().check_invariants(g,h)) @@ -40,7 +41,7 @@ TEST(Cyclic, Constructor) { //****************************************************************************** TEST(Cyclic, Compose) { EXPECT_LONGS_EQUAL(0, group::compose(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, group::compose(G(0),G(1))); + EXPECT_LONGS_EQUAL(1, group::compose(G(0),G(0))); EXPECT_LONGS_EQUAL(2, group::compose(G(0),G(2))); EXPECT_LONGS_EQUAL(3, group::compose(G(0),G(3))); EXPECT_LONGS_EQUAL(4, group::compose(G(0),G(4))); From 19c38b91ee460b7211066090002c0e2ae6564044 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 15:59:31 +0100 Subject: [PATCH 054/199] Finish CRTP in Cyclic --- gtsam/geometry/Cyclic.h | 47 ++++++++++++++++++++--------- gtsam/geometry/tests/testCyclic.cpp | 2 +- 2 files changed, 34 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index aded063e7..94d302d5b 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -20,21 +20,39 @@ namespace gtsam { -/// Additive Group +/// Additive Group, using CRTP template -class AdditiveGroup { - +struct AdditiveGroup { + static Derived Identity() { + return Derived::Identity(); + } + Derived const * derived() const { + return static_cast(this); + } + Derived operator+(const AdditiveGroup& h) const { + return derived()->operator+(*h.derived()); + } + Derived operator-(const AdditiveGroup& h) const { + return derived()->operator-(*h.derived()); + } + Derived operator-() const { + return derived()->operator-(); + } }; /// Cyclic group of order N template -class Cyclic : AdditiveGroup > { +class Cyclic : public AdditiveGroup > { size_t i_; ///< we just use an unsigned int public: /// Constructor Cyclic(size_t i) : i_(i) { } + // Idenity element + static Cyclic Identity() { + return Cyclic(0); + } /// Cast to size_t operator size_t() const { return i_; @@ -64,8 +82,9 @@ public: }; namespace traits { -/// Define Cyclic to be a model of the Group concept -template struct structure_category > { +/// Define any additive group to be at least a model of the Group concept +template +struct structure_category > { typedef group_tag type; }; } // \namespace gtsam::traits @@ -73,30 +92,30 @@ template struct structure_category > { namespace group { template -AdditiveGroup compose(const AdditiveGroup&g, const AdditiveGroup& h) { +G compose(const AdditiveGroup&g, const AdditiveGroup& h) { return g + h; } template -AdditiveGroup between(const AdditiveGroup&g, const AdditiveGroup& h) { +G between(const AdditiveGroup&g, const AdditiveGroup& h) { return h - g; } template -AdditiveGroup inverse(const AdditiveGroup&g) { +G inverse(const AdditiveGroup&g) { return -g; } namespace traits { /// Define the trait that specifies Cyclic's identity element -template struct identity > { - static const Cyclic value; - typedef Cyclic value_type; +template struct identity > { + static const G value; + typedef G value_type; }; -template -const Cyclic identity >::value = Cyclic(0); +template +const G identity >::value = AdditiveGroup::Identity(); /// Define the trait that asserts AdditiveGroup is an additive group template struct flavor > { diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 1a30beb1c..95ed1b5b9 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -27,7 +27,7 @@ typedef Cyclic<6> G; // Let's use the cyclic group of order 6 //****************************************************************************** TEST(Cyclic, Concept) { // BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((AdditiveGroup)); +// BOOST_CONCEPT_ASSERT((IsGroup >)); EXPECT_LONGS_EQUAL(0, group::traits::identity::value); G g(2), h(3); // EXPECT(Group().check_invariants(g,h)) From 38a084209074d2466be95fb51f5894a5a6f8e514 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 18:01:30 +0100 Subject: [PATCH 055/199] Macros. Too ugly? --- gtsam/base/concepts.h | 11 +++++++ gtsam/geometry/tests/testQuaternion.cpp | 41 +------------------------ 2 files changed, 12 insertions(+), 40 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 0ffb2fbd2..39e707a95 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -149,6 +149,17 @@ check_invariants(const T& a, const T& b, double tol = 1e-9) { } } // \ namespace group +#define GTSAM_MULTIPLICATIVE_GROUP2(T1,A1,T2,A2,GROUP) \ +namespace group { \ +template GROUP compose(const GROUP &g, const GROUP & h) { return g * h;} \ +template GROUP between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \ +template GROUP inverse(const GROUP &g) { return g.inverse();} \ +namespace traits { \ +template struct identity > { static const GROUP value; typedef GROUP value_type;};\ +template const GROUP identity >::value = GROUP::Identity();\ +template struct flavor > { typedef multiplicative_tag type;};\ +}} + /** * Group Concept */ diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 89dcd3024..eaef53230 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -111,47 +111,8 @@ struct DefaultChart > { } // \namespace gtsam::manifold::traits } // \namespace gtsam::manifold -namespace group { +GTSAM_MULTIPLICATIVE_GROUP2(typename,_Scalar, int,_Options ,Eigen::Quaternion) -template -Eigen::Quaternion compose(const Eigen::Quaternion &g, - const Eigen::Quaternion & h) { - return g * h; -} - -template -Eigen::Quaternion between(const Eigen::Quaternion &g, - const Eigen::Quaternion & h) { - return g.inverse() * h; -} - -template -Eigen::Quaternion inverse(const Eigen::Quaternion &g) { - return g.inverse(); -} - -namespace traits { - -/// Declare the trait that specifies a quaternion's identity element -template -struct identity > { - static const Eigen::Quaternion value; - typedef Eigen::Quaternion value_type; -}; - -/// Out of line definition of identity -template -const Eigen::Quaternion identity >::value = - Eigen::Quaternion::Identity(); - -/// Define the trait that asserts quaternions are a multiplicative group -template -struct flavor > { - typedef multiplicative_tag type; -}; - -} // \namespace gtsam::group::traits -} // \namespace gtsam::group } // \namespace gtsam /** From 9194b92cf66f966f255f6bc0cd4a925236d86dd5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 18:41:18 +0100 Subject: [PATCH 056/199] Now work with MACROS instead - but get linking error. Upside (if we can fix that): uniform treatment between foreign types (see Quaternion) and GTSAM types (Cyclic). Downside: seems I had to create a different macro for different number of template arguments. Help? --- gtsam/base/concepts.h | 11 +++++ gtsam/geometry/Cyclic.h | 66 +++-------------------------- gtsam/geometry/tests/testCyclic.cpp | 2 +- 3 files changed, 19 insertions(+), 60 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 39e707a95..3eabdd3ad 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -149,6 +149,17 @@ check_invariants(const T& a, const T& b, double tol = 1e-9) { } } // \ namespace group +#define GTSAM_ADDITIVE_GROUP1(T1,A1,GROUP) \ +namespace group { \ +template GROUP compose(const GROUP &g, const GROUP & h) { return g + h;} \ +template GROUP between(const GROUP &g, const GROUP & h) { return h - g;} \ +template GROUP inverse(const GROUP &g) { return -g;} \ +namespace traits { \ +template struct identity > { static const GROUP value; typedef GROUP value_type;};\ +template const GROUP identity >::value = GROUP::Identity();\ +template struct flavor > { typedef additive_tag type;};\ +}} + #define GTSAM_MULTIPLICATIVE_GROUP2(T1,A1,T2,A2,GROUP) \ namespace group { \ template GROUP compose(const GROUP &g, const GROUP & h) { return g * h;} \ diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 94d302d5b..f97f05dc8 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -20,34 +20,15 @@ namespace gtsam { -/// Additive Group, using CRTP -template -struct AdditiveGroup { - static Derived Identity() { - return Derived::Identity(); - } - Derived const * derived() const { - return static_cast(this); - } - Derived operator+(const AdditiveGroup& h) const { - return derived()->operator+(*h.derived()); - } - Derived operator-(const AdditiveGroup& h) const { - return derived()->operator-(*h.derived()); - } - Derived operator-() const { - return derived()->operator-(); - } -}; - /// Cyclic group of order N template -class Cyclic : public AdditiveGroup > { +class Cyclic { size_t i_; ///< we just use an unsigned int public: /// Constructor Cyclic(size_t i) : i_(i) { + assert(i -struct structure_category > { +/// Define cyclic group to be a model of the Group concept +template +struct structure_category > { typedef group_tag type; }; } // \namespace gtsam::traits -namespace group { - -template -G compose(const AdditiveGroup&g, const AdditiveGroup& h) { - return g + h; -} - -template -G between(const AdditiveGroup&g, const AdditiveGroup& h) { - return h - g; -} - -template -G inverse(const AdditiveGroup&g) { - return -g; -} - -namespace traits { - -/// Define the trait that specifies Cyclic's identity element -template struct identity > { - static const G value; - typedef G value_type; -}; - -template -const G identity >::value = AdditiveGroup::Identity(); - -/// Define the trait that asserts AdditiveGroup is an additive group -template struct flavor > { - typedef additive_tag type; -}; - -} // \namespace gtsam::group::traits -} // \namespace gtsam::group } // \namespace gtsam diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 95ed1b5b9..dfc94f36e 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -26,7 +26,7 @@ typedef Cyclic<6> G; // Let's use the cyclic group of order 6 //****************************************************************************** TEST(Cyclic, Concept) { -// BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); // BOOST_CONCEPT_ASSERT((IsGroup >)); EXPECT_LONGS_EQUAL(0, group::traits::identity::value); G g(2), h(3); From 481be105090ac02f8a6a1e38f85dd859496cc0eb Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 18:55:33 +0100 Subject: [PATCH 057/199] A bit more compact --- gtsam/base/concepts.h | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 3eabdd3ad..733f85696 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -192,16 +192,13 @@ public: } BOOST_CONCEPT_USAGE(IsGroup) { - using group::compose; - using group::between; - using group::inverse; BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::value), "This type's structure_category trait does not assert it as a group (or derived)"); e = group::traits::identity::value; - g = compose(g, h); - g = between(g, h); - g = inverse(g); + g = group::compose(g, h); + g = group::between(g, h); + g = group::inverse(g); operator_usage(flavor); } From 10ae9f5505c0e867bdbf01446292ef26a207235d Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 18:55:49 +0100 Subject: [PATCH 058/199] Easier to read with Cyclic<3> --- gtsam/geometry/Cyclic.cpp | 5 ---- gtsam/geometry/tests/testCyclic.cpp | 36 ++++++++--------------------- 2 files changed, 9 insertions(+), 32 deletions(-) diff --git a/gtsam/geometry/Cyclic.cpp b/gtsam/geometry/Cyclic.cpp index 2b14ec6ba..7242459a6 100644 --- a/gtsam/geometry/Cyclic.cpp +++ b/gtsam/geometry/Cyclic.cpp @@ -18,10 +18,5 @@ #include namespace gtsam { -namespace group { -namespace traits { - -} // \namespace traits -} // \namespace group } // \namespace gtsam diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index dfc94f36e..1b2f5bd7e 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -22,15 +22,12 @@ using namespace std; using namespace gtsam; -typedef Cyclic<6> G; // Let's use the cyclic group of order 6 +typedef Cyclic<3> G; // Let's use the cyclic group of order 3 //****************************************************************************** TEST(Cyclic, Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); -// BOOST_CONCEPT_ASSERT((IsGroup >)); EXPECT_LONGS_EQUAL(0, group::traits::identity::value); - G g(2), h(3); - // EXPECT(Group().check_invariants(g,h)) } //****************************************************************************** @@ -41,18 +38,12 @@ TEST(Cyclic, Constructor) { //****************************************************************************** TEST(Cyclic, Compose) { EXPECT_LONGS_EQUAL(0, group::compose(G(0),G(0))); - EXPECT_LONGS_EQUAL(1, group::compose(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, group::compose(G(0),G(1))); EXPECT_LONGS_EQUAL(2, group::compose(G(0),G(2))); - EXPECT_LONGS_EQUAL(3, group::compose(G(0),G(3))); - EXPECT_LONGS_EQUAL(4, group::compose(G(0),G(4))); - EXPECT_LONGS_EQUAL(5, group::compose(G(0),G(5))); EXPECT_LONGS_EQUAL(2, group::compose(G(2),G(0))); - EXPECT_LONGS_EQUAL(3, group::compose(G(2),G(1))); - EXPECT_LONGS_EQUAL(4, group::compose(G(2),G(2))); - EXPECT_LONGS_EQUAL(5, group::compose(G(2),G(3))); - EXPECT_LONGS_EQUAL(0, group::compose(G(2),G(4))); - EXPECT_LONGS_EQUAL(1, group::compose(G(2),G(5))); + EXPECT_LONGS_EQUAL(0, group::compose(G(2),G(1))); + EXPECT_LONGS_EQUAL(1, group::compose(G(2),G(2))); } //****************************************************************************** @@ -60,32 +51,23 @@ TEST(Cyclic, Between) { EXPECT_LONGS_EQUAL(0, group::between(G(0),G(0))); EXPECT_LONGS_EQUAL(1, group::between(G(0),G(1))); EXPECT_LONGS_EQUAL(2, group::between(G(0),G(2))); - EXPECT_LONGS_EQUAL(3, group::between(G(0),G(3))); - EXPECT_LONGS_EQUAL(4, group::between(G(0),G(4))); - EXPECT_LONGS_EQUAL(5, group::between(G(0),G(5))); - EXPECT_LONGS_EQUAL(4, group::between(G(2),G(0))); - EXPECT_LONGS_EQUAL(5, group::between(G(2),G(1))); + EXPECT_LONGS_EQUAL(1, group::between(G(2),G(0))); + EXPECT_LONGS_EQUAL(2, group::between(G(2),G(1))); EXPECT_LONGS_EQUAL(0, group::between(G(2),G(2))); - EXPECT_LONGS_EQUAL(1, group::between(G(2),G(3))); - EXPECT_LONGS_EQUAL(2, group::between(G(2),G(4))); - EXPECT_LONGS_EQUAL(3, group::between(G(2),G(5))); } //****************************************************************************** TEST(Cyclic, Ivnverse) { EXPECT_LONGS_EQUAL(0, group::inverse(G(0))); - EXPECT_LONGS_EQUAL(5, group::inverse(G(1))); - EXPECT_LONGS_EQUAL(4, group::inverse(G(2))); - EXPECT_LONGS_EQUAL(3, group::inverse(G(3))); - EXPECT_LONGS_EQUAL(2, group::inverse(G(4))); - EXPECT_LONGS_EQUAL(1, group::inverse(G(5))); + EXPECT_LONGS_EQUAL(2, group::inverse(G(1))); + EXPECT_LONGS_EQUAL(1, group::inverse(G(2))); } //****************************************************************************** TEST(Cyclic , Invariants) { G g(2), h(5); - group::check_invariants(g,h); +// group::check_invariants(g,h); } //****************************************************************************** From 62ae58e7ea403cb71797925a3409237d82295acc Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 19:13:02 +0100 Subject: [PATCH 059/199] Quaternion header --- gtsam/geometry/Quaternion.h | 124 ++++++++++++++++++++++++ gtsam/geometry/tests/testQuaternion.cpp | 121 +---------------------- 2 files changed, 127 insertions(+), 118 deletions(-) create mode 100644 gtsam/geometry/Quaternion.h diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h new file mode 100644 index 000000000..dce47beca --- /dev/null +++ b/gtsam/geometry/Quaternion.h @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 Unit tests for unit quaternions + * @author Frank Dellaert + **/ + +#include + +namespace gtsam { + +namespace traits { + +/// Define Eigen::Quaternion to be a model of the Lie Group concept +template +struct structure_category > { + typedef lie_group_tag type; +}; + +} // \namespace gtsam::traits + +namespace manifold { + +/// Chart for Eigen Quaternions +template +struct QuaternionChart { + + // required + typedef Eigen::Quaternion ManifoldType; + + // internal + typedef ManifoldType Q; + typedef typename traits::TangentVector::type Omega; + + /// Exponential map, simply be converting omega to AngleAxis + static Q Expmap(const Omega& omega) { + double theta = omega.norm(); + if (std::abs(theta) < 1e-10) + return Q::Identity(); + return Q(Eigen::AngleAxisd(theta, omega / theta)); + } + + /// retract, simply be converting omega to AngleAxis + static Q Retract(const Q& p, const Omega& omega) { + return p * Expmap(omega); + } + + /// We use our own Logmap, as there is a slight bug in Eigen + static Omega Logmap(const Q& q) { + 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 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(); + } + } + + /// local is our own, as there is a slight bug in Eigen + static Omega Local(const Q& q1, const Q& q2) { + return Logmap(q1.inverse() * q2); + } +}; + +namespace traits { + +/// Define the trait that asserts Quaternion manifold has dimension 3 +template +struct dimension > : public boost::integral_constant< + int, 3> { +}; + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct TangentVector > { + typedef Eigen::Matrix type; +}; + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct DefaultChart > { + typedef QuaternionChart type; +}; + +} // \namespace gtsam::manifold::traits +} // \namespace gtsam::manifold + +GTSAM_MULTIPLICATIVE_GROUP2(typename,_Scalar, int,_Options ,Eigen::Quaternion) + +} // \namespace gtsam + +/** + * GSAM typedef to an Eigen::Quaternion, 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 Quaternion; + diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index eaef53230..534fe155a 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -10,127 +10,12 @@ * -------------------------------------------------------------------------- */ /** - * @file Quaternion.h - * @brief Unit tests for unit quaternions + * @file testQuaternion.cpp + * @brief Unit tests for Quaternion, as a GTSAM-adapted Lie Group * @author Frank Dellaert **/ -#include - -namespace gtsam { - -namespace traits { - -/// Define Eigen::Quaternion to be a model of the Lie Group concept -template -struct structure_category > { - typedef lie_group_tag type; -}; - -} // \namespace gtsam::traits - -namespace manifold { - -/// Chart for Eigen Quaternions -template -struct QuaternionChart { - - // required - typedef Eigen::Quaternion ManifoldType; - - // internal - typedef ManifoldType Q; - typedef typename traits::TangentVector::type Omega; - - /// Exponential map, simply be converting omega to AngleAxis - static Q Expmap(const Omega& omega) { - double theta = omega.norm(); - if (std::abs(theta) < 1e-10) - return Q::Identity(); - return Q(Eigen::AngleAxisd(theta, omega / theta)); - } - - /// retract, simply be converting omega to AngleAxis - static Q Retract(const Q& p, const Omega& omega) { - return p * Expmap(omega); - } - - /// We use our own Logmap, as there is a slight bug in Eigen - static Omega Logmap(const Q& q) { - 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 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(); - } - } - - /// local is our own, as there is a slight bug in Eigen - static Omega Local(const Q& q1, const Q& q2) { - return Logmap(q1.inverse() * q2); - } -}; - -namespace traits { - -/// Define the trait that asserts Quaternion manifold has dimension 3 -template -struct dimension > : public boost::integral_constant< - int, 3> { -}; - -/// Define the trait that asserts Quaternion TangentVector is Vector3 -template -struct TangentVector > { - typedef Eigen::Matrix type; -}; - -/// Define the trait that asserts Quaternion TangentVector is Vector3 -template -struct DefaultChart > { - typedef QuaternionChart type; -}; - -} // \namespace gtsam::manifold::traits -} // \namespace gtsam::manifold - -GTSAM_MULTIPLICATIVE_GROUP2(typename,_Scalar, int,_Options ,Eigen::Quaternion) - -} // \namespace gtsam - -/** - * GSAM typedef to an Eigen::Quaternion, 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 Quaternion; - -/** - * @file testCyclic.cpp - * @brief Unit tests for cyclic group - * @author Frank Dellaert - **/ - -//#include -#include -#include +#include #include using namespace std; From 04d3457b45fa436b944ac87155238a96a51d218c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 19:30:46 +0100 Subject: [PATCH 060/199] First working Manifold macro --- gtsam/geometry/Cyclic.h | 6 ++-- gtsam/geometry/Quaternion.h | 55 +++++++++++++------------------------ 2 files changed, 22 insertions(+), 39 deletions(-) diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index f97f05dc8..cef085374 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -62,15 +62,15 @@ public: }; -GTSAM_ADDITIVE_GROUP1(size_t,N,Cyclic) +GTSAM_ADDITIVE_GROUP1(size_t, N, Cyclic) -namespace traits { /// Define cyclic group to be a model of the Group concept +namespace traits { template struct structure_category > { typedef group_tag type; }; -} // \namespace gtsam::traits +} } // \namespace gtsam diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index dce47beca..79e71da3b 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -19,18 +19,6 @@ namespace gtsam { -namespace traits { - -/// Define Eigen::Quaternion to be a model of the Lie Group concept -template -struct structure_category > { - typedef lie_group_tag type; -}; - -} // \namespace gtsam::traits - -namespace manifold { - /// Chart for Eigen Quaternions template struct QuaternionChart { @@ -40,7 +28,7 @@ struct QuaternionChart { // internal typedef ManifoldType Q; - typedef typename traits::TangentVector::type Omega; + typedef typename manifold::traits::TangentVector::type Omega; /// Exponential map, simply be converting omega to AngleAxis static Q Expmap(const Omega& omega) { @@ -88,32 +76,25 @@ struct QuaternionChart { } }; +#define GTSAM_MANIFOLD2(T1,A1,T2,A2,MANIFOLD,DIM,SCALAR,OPTIONS,CHART) \ +namespace manifold { \ +namespace traits { \ +template struct dimension > : public boost::integral_constant {};\ +template struct TangentVector > { typedef Eigen::Matrix type;};\ +template struct DefaultChart > { typedef CHART type;};\ +}} + + +GTSAM_MANIFOLD2(typename, _Scalar, int, _Options, Eigen::Quaternion, 3, _Scalar, _Options, QuaternionChart) +GTSAM_MULTIPLICATIVE_GROUP2(typename, _Scalar, int, _Options, Eigen::Quaternion) + +/// Define Eigen::Quaternion to be a model of the Lie Group concept namespace traits { - -/// Define the trait that asserts Quaternion manifold has dimension 3 template -struct dimension > : public boost::integral_constant< - int, 3> { +struct structure_category > { + typedef lie_group_tag type; }; - -/// Define the trait that asserts Quaternion TangentVector is Vector3 -template -struct TangentVector > { - typedef Eigen::Matrix type; -}; - -/// Define the trait that asserts Quaternion TangentVector is Vector3 -template -struct DefaultChart > { - typedef QuaternionChart type; -}; - -} // \namespace gtsam::manifold::traits -} // \namespace gtsam::manifold - -GTSAM_MULTIPLICATIVE_GROUP2(typename,_Scalar, int,_Options ,Eigen::Quaternion) - -} // \namespace gtsam +} /** * GSAM typedef to an Eigen::Quaternion, we disable alignment because @@ -122,3 +103,5 @@ GTSAM_MULTIPLICATIVE_GROUP2(typename,_Scalar, int,_Options ,Eigen::Quaternion) */ typedef Eigen::Quaternion Quaternion; +} // \namespace gtsam + From 949a6b7f4cfee54a869cf3161b2b7ca435055c1f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 19:55:06 +0100 Subject: [PATCH 061/199] Found solution for template arguments in macros. Requires a bit more from caller, but is better. I quite like the MACROS: It's compact... --- gtsam/base/concepts.h | 36 ++++++++++++++++++++++-------------- 1 file changed, 22 insertions(+), 14 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 733f85696..119d3b16a 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -69,6 +69,14 @@ check_invariants(const T& a, const T& b) { } // \ namespace manifold +#define GTSAM_MANIFOLD(TEMPLATE,MANIFOLD,DIM,TANGENT_VECTOR,DEFAULT_CHART) \ +namespace manifold { \ +namespace traits { \ +template