diff --git a/.cproject b/.cproject index 9d2b1dca0..dafdeed71 100644 --- a/.cproject +++ b/.cproject @@ -592,6 +592,7 @@ make + tests/testBayesTree.run true false @@ -599,6 +600,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +648,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +656,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +664,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +680,7 @@ make + tests/testBayesTree true false @@ -1130,6 +1136,7 @@ make + testErrors.run true false @@ -1359,6 +1366,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 +1488,6 @@ make - testSimulated2DOriented.run true false @@ -1481,7 +1527,6 @@ make - testSimulated2D.run true false @@ -1489,7 +1534,6 @@ make - testSimulated3D.run true false @@ -1503,46 +1547,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 +1804,7 @@ cpack + -G DEB true false @@ -1807,6 +1812,7 @@ cpack + -G RPM true false @@ -1814,6 +1820,7 @@ cpack + -G TGZ true false @@ -1821,6 +1828,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2185,6 +2193,14 @@ true true + + make + -j4 + testSO3.run + true + true + true + make -j2 @@ -2683,6 +2699,7 @@ make + testGraph.run true false @@ -2690,6 +2707,7 @@ make + testJunctionTree.run true false @@ -2697,6 +2715,7 @@ make + testSymbolicBayesNetB.run true false @@ -3240,7 +3259,6 @@ make - tests/testGaussianISAM2 true false diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md new file mode 100644 index 000000000..8faf7a4a8 --- /dev/null +++ b/GTSAM-Concepts.md @@ -0,0 +1,201 @@ +GTSAM Concepts +============== + +As discussed in [Generic Programming Techniques](http://www.boost.org/community/generic_programming.html), concepts define + +* associated types +* valid expressions, like functions and values +* invariants +* complexity guarantees + +Below we discuss the most important concepts use in GTSAM, and after that we discuss how they are implemented/used/enforced. + + +Manifold +-------- + +To optimize over continuous types, we assume they are manifolds. This is central to GTSAM and hence discussed in some more detail below. + +[Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space. + +In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented. +In detail, we ask the following are defined in the traits object: + +* values: + * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimenionality is only defined at runtime, by specifying the value -1. +* types: + * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. + * `ChartJacobian`, a typedef for `OptionalJacobian`. + * `ManifoldType`, a pointer back to the type. + * `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following: + * `gtsam::traits::manifold_tag` -- Everything in this list is expected + * `gtsam::traits::group_tag` -- The functions defined under **Groups** below. + * `gtsam::traits::lie_group_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below. + * `gtsam::traits::vector_space_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below. +* valid expressions: + * `size_t dim = traits::getDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time. + * `v = traits::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p*, where *p* and *q* are elements of the manifold and the result, *v* is an element of the vector space. + * `p = traits::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v*, where *p* is an element of the manifold and the result, *v* is an element of the vector space. +* invariants + * `Retract(p, Local(p,q)) == q` + * `Local(p, Retract(p, v)) == v` + +Group +----- +A [group]("http://en.wikipedia.org/wiki/Group_(mathematics)"") should be well known from grade school :-), and provides a type with a composition operation that is closed, associative, has an identity element, and an inverse for each element. The following should be added to the traits class for a group: + +* valid expressions: + * `r = traits::Compose(p,q)`, where *p*, *q*, and *r* are elements of the manifold. + * `q = traits::Inverse(p)`, where *p* and*q* are elements of the manifold. + * `r = traits::Between(p,q)`, where *p*, *q*, and *r* are elements of the manifold. +* static members: + * `traits::Identity`, a static const member that represents the group's identity element. +* invariants: + * `Compose(p,Inverse(p)) == Identity` + * `Compose(p,Between(p,q)) == q` + * `Between(p,q) == Compose(Inverse(p),q)` +The `gtsam::group::traits` namespace defines the following: +* values: + * `traits::Identity` -- The identity element for this group stored as a static const. + * `traits::group_flavor` -- the flavor of this group's `compose()` operator, either: + * `gtsam::traits::group_multiplicative_tag` for multiplicative operator syntax ,or + * `gtsam::traits::group_additive_tag` for additive operator syntax. + +We do *not* at this time support more than one composition operator per type. Although mathematically possible, it is hardly ever needed, and the machinery to support it would be burdensome and counter-intuitive. + +Also, a type should provide either multiplication or addition operators depending on the flavor of the operation. To distinguish between the two, we will use a tag (see below). + +Group Action +------------ + +Group actions are concepts in and of themselves that can be concept checked (see below). +In particular, a group can *act* on another space. +For example, the [cyclic group of order 6](http://en.wikipedia.org/wiki/Cyclic_group) can rotate 2D vectors around the origin: + + q = R(i)*p + where R(i) = R(60)^i, where R(60) rotates by 60 degrees + +Hence, we formalize by the following extension of the concept: + +* valid expressions: + * `q = traits::Act(g,p)`, for some instance, *p*, of a space *S*, that can be acted upon by the group element *g* to produce *q* in *S*. + * `q = traits::Act(g,p,Hp)`, if the space acted upon is a continuous differentiable manifold. * + +In the latter case, if *S* is an n-dimensional manifold, *Hp* is an output argument that should be +filled with the *nxn* Jacobian matrix of the action with respect to a change in *p*. It typically depends +on the group element *g*, but in most common example will *not* depend on the value of *p*. For example, in +the cyclic group example above, we simply have + + Hp = R(i) + +Note there is no derivative of the action with respect to a change in g. That will only be defined +for Lie groups, which we introduce now. + +Lie Group +--------- + +A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should implements both MANIFOLD and GROUP concepts. +However, we now also need to be able to evaluate the derivatives of compose and inverse. +Hence, we have the following extra valid static functions defined in the struct `gtsam::traits`: + +* `r = traits::Compose(p,q,Hq,Hp)` +* `q = traits::Inverse(p,Hp)` +* `r = traits::Between(p,q,Hq,H2p)` + +where above the *H* arguments stand for optional Jacobian arguments. +That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor). + +In addition, a Lie group has a Lie algebra, which affords two extra valid expressions: + +* `v = traits::Logmap(p,Hp)`, the log map, with optional Jacobian +* `p = traits::Expmap(v,Hv)`, the exponential map, with optional Jacobian + +Note that in the Lie group case, the usual valid expressions for Retract and Local can be generated automatically, e.g. + +``` + T Retract(p,v,Hp,Hv) { + T q = Expmap(v,Hqv); + T r = Compose(p,q,Hrp,Hrq); + Hv = Hrq * Hqv; // chain rule + return r; + } +``` + +For Lie groups, the `exponential map` above is the most obvious mapping: it +associates straight lines in the tangent space with geodesics on the manifold +(and it's inverse, the log map). However, there are two cases in which we deviate from this: + +However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`) + +Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method. + +Lie Group Action +---------------- + +When a Lie group acts on a space, we have two derivatives to care about: + + * `gtasm::manifold::traits::act(g,p,Hg,Hp)`, if the space acted upon is a continuous differentiable manifold. + +An example is a *similarity transform* in 3D, which can act on 3D space, like + + q = s*R*p + t + +Note that again the derivative in *p*, *Hp* is simply *s R*, which depends on *g* but not on *p*. +The derivative in *g*, *Hg*, is in general more complex. + +For now, we won't care about Lie groups acting on non-manifolds. + +Vector Space +------------ + +While vector spaces are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. I.e.where + + * `Identity == 0` + * `Inverse(p) == -p` + * `Compose(p,q) == p+q` + * `Between(p,q) == q-p` + * `Local(q) == p-q` + * `Retract(v) == p+v` + +This considerably simplifies certain operations. A `VectorSpace` superclass is available to implement the traits. Types that are vector space models include `Matrix`, `Vector`, any fixed or dynamic Eigen Matrix, `Point2`, and `Point3`. + +Testable +-------- +Unit tests heavily depend on the following two functions being defined for all types that need to be tested: + +* valid expressions: + * `Print(p,s)` where s is an optional string + * `Equals(p,q,tol)` where tol is an optional (double) tolerance + +Implementation +============== + +GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the +TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts. + +`gtsam::traits` is our way to associate these concepts with types, +and we also define a limited number of `gtsam::tags` to select the correct implementation +of certain functions at compile time (tag dispatching). + +Traits +------ + +However, a base class is not a good way to implement/check the other concepts, as we would like these +to apply equally well to types that are outside GTSAM control, e.g., `Eigen::VectorXd`. This is where +[traits](http://www.boost.org/doc/libs/1_57_0/libs/type_traits/doc/html/boost_typetraits/background.html) come in. + +We use Eigen-style or STL-style traits, that define *many* properties at once. + +Note that not everything that makes a concept is defined by traits. Valid expressions such as traits::Compose are +defined simply as static functions within the traits class. +Finally, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types, +rather than having to use traits internally. + +Concept Checks +-------------- + +Boost provides a nice way to check whether a given type satisfies a concept. For example, the following + + BOOST_CONCEPT_ASSERT(IsVectorSpace) + +asserts that Point2 indeed is a model for the VectorSpace concept. \ No newline at end of file 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..9c06a5d28 100644 --- a/THANKS +++ b/THANKS @@ -1,20 +1,43 @@ -GTSAM was made possible by the efforts of many collaborators at Georgia Tech +GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech: -Doru Balcan -Chris Beall -Alex Cunningham -Alireza Fathi -Eohan George -Viorela Ila -Yong-Dian Jian -Michael Kaess -Kai Ni -Carlos Nieto -Duy-Nguyen -Manohar Paluri -Christian Potthast -Richard Roberts -Grant Schindler +* Sungtae An +* Doru Balcan, Bank of America +* Chris Beall +* Luca Carlone +* Alex Cunningham, U Michigan +* Jing Dong +* Alireza Fathi, Stanford +* Eohan George +* Alex Hagiopol +* Viorela Ila, Czeck Republic +* Vadim Indelman, the Technion +* David Jensen, GTRI +* Yong-Dian Jian, Baidu +* Michael Kaess, Carnegie Mellon +* Zhaoyang Lv +* Andrew Melim, Oculus Rift +* Kai Ni, Baidu +* Carlos Nieto +* Duy-Nguyen Ta +* Manohar Paluri, Facebook +* Christian Potthast, USC +* Richard Roberts, Google X +* Grant Schindler, Consultant +* Natesh Srinivasan +* Alex Trevor +* Stephen Williams, BossaNova + +at ETH, Zurich + +* Paul Furgale +* Mike Bosse +* Hannes Sommer +* Thomas Schneider + +at Uni Zurich: + +* Christian Forster Many thanks for your hard work!!!! + Frank Dellaert diff --git a/USAGE b/USAGE.md similarity index 57% rename from USAGE rename to USAGE.md index 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 diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 3dd978ee3..018a15dd8 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -72,22 +72,22 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -BOOST_CLASS_EXPORT(Value); -BOOST_CLASS_EXPORT(Pose); -BOOST_CLASS_EXPORT(Rot2); -BOOST_CLASS_EXPORT(Point2); -BOOST_CLASS_EXPORT(NonlinearFactor); -BOOST_CLASS_EXPORT(NoiseModelFactor); -BOOST_CLASS_EXPORT(NM1); -BOOST_CLASS_EXPORT(NM2); -BOOST_CLASS_EXPORT(BetweenFactor); -BOOST_CLASS_EXPORT(PriorFactor); -BOOST_CLASS_EXPORT(BR); -BOOST_CLASS_EXPORT(noiseModel::Base); -BOOST_CLASS_EXPORT(noiseModel::Isotropic); -BOOST_CLASS_EXPORT(noiseModel::Gaussian); -BOOST_CLASS_EXPORT(noiseModel::Diagonal); -BOOST_CLASS_EXPORT(noiseModel::Unit); +GTSAM_VALUE_EXPORT(Value); +GTSAM_VALUE_EXPORT(Pose); +GTSAM_VALUE_EXPORT(Rot2); +GTSAM_VALUE_EXPORT(Point2); +GTSAM_VALUE_EXPORT(NonlinearFactor); +GTSAM_VALUE_EXPORT(NoiseModelFactor); +GTSAM_VALUE_EXPORT(NM1); +GTSAM_VALUE_EXPORT(NM2); +GTSAM_VALUE_EXPORT(BetweenFactor); +GTSAM_VALUE_EXPORT(PriorFactor); +GTSAM_VALUE_EXPORT(BR); +GTSAM_VALUE_EXPORT(noiseModel::Base); +GTSAM_VALUE_EXPORT(noiseModel::Isotropic); +GTSAM_VALUE_EXPORT(noiseModel::Gaussian); +GTSAM_VALUE_EXPORT(noiseModel::Diagonal); +GTSAM_VALUE_EXPORT(noiseModel::Unit); double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { // Compute degrees of freedom (observations - variables) diff --git a/gtsam.h b/gtsam.h index 4cccea87b..1fbc0f907 100644 --- a/gtsam.h +++ b/gtsam.h @@ -270,8 +270,6 @@ class Point2 { gtsam::Point2 between(const gtsam::Point2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Point2 retract(Vector v) const; Vector localCoordinates(const gtsam::Point2& p) const; @@ -342,8 +340,6 @@ class Point3 { gtsam::Point3 between(const gtsam::Point3& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Point3 retract(Vector v) const; Vector localCoordinates(const gtsam::Point3& p) const; @@ -380,8 +376,6 @@ class Rot2 { gtsam::Rot2 between(const gtsam::Rot2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Rot2 retract(Vector v) const; Vector localCoordinates(const gtsam::Rot2& p) const; @@ -433,8 +427,6 @@ class Rot3 { gtsam::Rot3 between(const gtsam::Rot3& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options gtsam::Rot3 retract(Vector v) const; Vector localCoordinates(const gtsam::Rot3& p) const; @@ -482,8 +474,6 @@ class Pose2 { gtsam::Pose2 between(const gtsam::Pose2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Pose2 retract(Vector v) const; Vector localCoordinates(const gtsam::Pose2& p) const; @@ -531,10 +521,7 @@ class Pose3 { gtsam::Pose3 between(const gtsam::Pose3& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Pose3 retract(Vector v) const; - gtsam::Pose3 retractFirstOrder(Vector v) const; Vector localCoordinates(const gtsam::Pose3& T2) const; // Lie Group @@ -2384,8 +2371,6 @@ class ConstantBias { gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::imuBias::ConstantBias retract(Vector v) const; Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index 3cc6a041c..7c95592e1 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -1,221 +1,222 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file ChartValue.h - * @brief - * @date October, 2014 - * @author Michael Bosse, Abel Gawel, Renaud Dube - * based on DerivedValue.h by Duy Nguyen Ta - */ - +///* ---------------------------------------------------------------------------- +// +// * GTSAM Copyright 2010, Georgia Tech Research Corporation, +// * Atlanta, Georgia 30332-0415 +// * All Rights Reserved +// * Authors: Frank Dellaert, et al. (see THANKS for the full author list) +// +// * See LICENSE for the license information +// +// * -------------------------------------------------------------------------- */ +// +///* +// * @file ChartValue.h +// * @brief +// * @date October, 2014 +// * @author Michael Bosse, Abel Gawel, Renaud Dube +// * based on DerivedValue.h by Duy Nguyen Ta +// */ +// #pragma once - +// +#include #include -#include -#include - -////////////////// -// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR -#include - -#ifdef min -#undef min -#endif - -#ifdef max -#undef max -#endif - -#ifdef ERROR -#undef ERROR -#endif -////////////////// - -namespace gtsam { - -/** - * ChartValue is derived from GenericValue and Chart so that - * Chart can be zero sized (as in DefaultChart) - * if the Chart is a member variable then it won't ever be zero sized. - */ -template > -class ChartValue: public GenericValue, public Chart_ { - - BOOST_CONCEPT_ASSERT((ChartConcept)); - -public: - - typedef T type; - typedef Chart_ Chart; - -public: - - /// Default Constructor. TODO might not make sense for some types - ChartValue() : - GenericValue(T()) { - } - - /// Construct froma value - ChartValue(const T& value) : - GenericValue(value) { - } - - /// Construct from a value and initialize the chart - template - ChartValue(const T& value, C chart_initializer) : - GenericValue(value), Chart(chart_initializer) { - } - - /// Destructor - virtual ~ChartValue() { - } - - /** - * Create a duplicate object returned as a pointer to the generic Value interface. - * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. - * The result must be deleted with Value::deallocate_, not with the 'delete' operator. - */ - virtual Value* clone_() const { - void *place = boost::singleton_pool::malloc(); - ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in - return ptr; - } - - /** - * Destroy and deallocate this object, only if it was originally allocated using clone_(). - */ - virtual void deallocate_() const { - this->~ChartValue(); // Virtual destructor cleans up the derived object - boost::singleton_pool::free((void*) this); // Release memory from pool - } - - /** - * Clone this value (normal clone on the heap, delete with 'delete' operator) - */ - virtual boost::shared_ptr clone() const { - return boost::make_shared(*this); - } - - /// Chart Value interface version of retract - virtual Value* retract_(const Vector& delta) const { - // Call retract on the derived class using the retract trait function - const T retractResult = Chart::retract(GenericValue::value(), delta); - - // Create a Value pointer copy of the result - void* resultAsValuePlace = - boost::singleton_pool::malloc(); - Value* resultAsValue = new (resultAsValuePlace) ChartValue(retractResult, - static_cast(*this)); - - // Return the pointer to the Value base class - return resultAsValue; - } - - /// Generic Value interface version of localCoordinates - virtual Vector localCoordinates_(const Value& value2) const { - // Cast the base class Value pointer to a templated generic class pointer - const GenericValue& genericValue2 = - static_cast&>(value2); - - // Return the result of calling localCoordinates trait on the derived class - return Chart::local(GenericValue::value(), genericValue2.value()); - } - - /// Non-virtual version of retract - ChartValue retract(const Vector& delta) const { - return ChartValue(Chart::retract(GenericValue::value(), delta), - static_cast(*this)); - } - - /// Non-virtual version of localCoordinates - Vector localCoordinates(const ChartValue& value2) const { - return localCoordinates_(value2); - } - - /// Return run-time dimensionality - virtual size_t dim() const { - // need functional form here since the dimension may be dynamic - return Chart::getDimension(GenericValue::value()); - } - - /// Assignment operator - virtual Value& operator=(const Value& rhs) { - // Cast the base class Value pointer to a derived class pointer - const ChartValue& derivedRhs = static_cast(rhs); - - // Do the assignment and return the result - *this = ChartValue(derivedRhs); // calls copy constructor - return *this; - } - -protected: - - // implicit assignment operator for (const ChartValue& rhs) works fine here - /// Assignment operator, protected because only the Value or DERIVED - /// assignment operators should be used. - // DerivedValue& operator=(const DerivedValue& rhs) { - // // Nothing to do, do not call base class assignment operator - // return *this; - // } - -private: - - /// Fake Tag struct for singleton pool allocator. In fact, it is never used! - struct PoolTag { - }; - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - // ar & boost::serialization::make_nvp("value",); - // todo: implement a serialization for charts - ar - & boost::serialization::make_nvp("GenericValue", - boost::serialization::base_object >(*this)); - } - -}; - -// Define -namespace traits { - -/// The dimension of a ChartValue is the dimension of the chart -template -struct dimension > : public dimension { - // TODO Frank thinks dimension is a property of type, chart should conform -}; - -} // \ traits - -/// Get the chart from a Value -template -const Chart& Value::getChart() const { - return dynamic_cast(*this); -} - -/// Convenience function that can be used to make an expression to convert a value to a chart -template -ChartValue convertToChartValue(const T& value, - boost::optional< - Eigen::Matrix::value, - traits::dimension::value>&> H = boost::none) { - if (H) { - *H = Eigen::Matrix::value, - traits::dimension::value>::Identity(); - } - return ChartValue(value); -} - -} /* namespace gtsam */ +//#include +//#include +// +//////////////////// +//// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR +//#include +// +//#ifdef min +//#undef min +//#endif +// +//#ifdef max +//#undef max +//#endif +// +//#ifdef ERROR +//#undef ERROR +//#endif +//////////////////// +// +//namespace gtsam { +// +///** +// * ChartValue is derived from GenericValue and Chart so that +// * Chart can be zero sized (as in DefaultChart) +// * if the Chart is a member variable then it won't ever be zero sized. +// */ +//template > +//class GenericValue: public GenericValue, public Chart_ { +// +// BOOST_CONCEPT_ASSERT((ChartConcept)); +// +//public: +// +// typedef T type; +// typedef Chart_ Chart; +// +//public: +// +// /// Default Constructor. TODO might not make sense for some types +// GenericValue() : +// GenericValue(T()) { +// } +// +// /// Construct froma value +// GenericValue(const T& value) : +// GenericValue(value) { +// } +// +// /// Construct from a value and initialize the chart +// template +// GenericValue(const T& value, C chart_initializer) : +// GenericValue(value), Chart(chart_initializer) { +// } +// +// /// Destructor +// virtual ~ChartValue() { +// } +// +// /** +// * Create a duplicate object returned as a pointer to the generic Value interface. +// * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. +// * The result must be deleted with Value::deallocate_, not with the 'delete' operator. +// */ +// virtual Value* clone_() const { +// void *place = boost::singleton_pool::malloc(); +// ChartValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in +// return ptr; +// } +// +// /** +// * Destroy and deallocate this object, only if it was originally allocated using clone_(). +// */ +// virtual void deallocate_() const { +// this->~ChartValue(); // Virtual destructor cleans up the derived object +// boost::singleton_pool::free((void*) this); // Release memory from pool +// } +// +// /** +// * Clone this value (normal clone on the heap, delete with 'delete' operator) +// */ +// virtual boost::shared_ptr clone() const { +// return boost::make_shared(*this); +// } +// +// /// Chart Value interface version of retract +// virtual Value* retract_(const Vector& delta) const { +// // Call retract on the derived class using the retract trait function +// const T retractResult = Chart::retract(GenericValue::value(), delta); +// +// // Create a Value pointer copy of the result +// void* resultAsValuePlace = +// boost::singleton_pool::malloc(); +// Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult, +// static_cast(*this)); +// +// // Return the pointer to the Value base class +// return resultAsValue; +// } +// +// /// Generic Value interface version of localCoordinates +// virtual Vector localCoordinates_(const Value& value2) const { +// // Cast the base class Value pointer to a templated generic class pointer +// const GenericValue& genericValue2 = +// static_cast&>(value2); +// +// // Return the result of calling localCoordinates trait on the derived class +// return Chart::local(GenericValue::value(), genericValue2.value()); +// } +// +// /// Non-virtual version of retract +// GenericValue retract(const Vector& delta) const { +// return GenericValue(Chart::retract(GenericValue::value(), delta), +// static_cast(*this)); +// } +// +// /// Non-virtual version of localCoordinates +// Vector localCoordinates(const GenericValue& value2) const { +// return localCoordinates_(value2); +// } +// +// /// Return run-time dimensionality +// virtual size_t dim() const { +// // need functional form here since the dimension may be dynamic +// return Chart::getDimension(GenericValue::value()); +// } +// +// /// Assignment operator +// virtual Value& operator=(const Value& rhs) { +// // Cast the base class Value pointer to a derived class pointer +// const GenericValue& derivedRhs = static_cast(rhs); +// +// // Do the assignment and return the result +// *this = GenericValue(derivedRhs); // calls copy constructor +// return *this; +// } +// +//protected: +// +// // implicit assignment operator for (const ChartValue& rhs) works fine here +// /// Assignment operator, protected because only the Value or DERIVED +// /// assignment operators should be used. +// // DerivedValue& operator=(const DerivedValue& rhs) { +// // // Nothing to do, do not call base class assignment operator +// // return *this; +// // } +// +//private: +// +// /// Fake Tag struct for singleton pool allocator. In fact, it is never used! +// struct PoolTag { +// }; +// +//private: +// +// /** Serialization function */ +// friend class boost::serialization::access; +// template +// void serialize(ARCHIVE & ar, const unsigned int version) { +// // ar & boost::serialization::make_nvp("value",); +// // todo: implement a serialization for charts +// ar +// & boost::serialization::make_nvp("GenericValue", +// boost::serialization::base_object >(*this)); +// } +// +//}; +// +//// Define +//namespace traits { +// +///// The dimension of a GenericValue is the dimension of the chart +//template +//struct dimension > : public dimension { +// // TODO Frank thinks dimension is a property of type, chart should conform +//}; +// +//} // \ traits +// +///// Get the chart from a Value +//template +//const Chart& Value::getChart() const { +// return dynamic_cast(*this); +//} +// +///// Convenience function that can be used to make an expression to convert a value to a chart +//template +//GenericValue convertToChartValue(const T& value, +// boost::optional< +// Eigen::Matrix::dimension, +// traits::dimension>&> H = boost::none) { +// if (H) { +// *H = Eigen::Matrix::dimension, +// traits::dimension>::Identity(); +// } +// return GenericValue(value); +//} +// +//} /* namespace gtsam */ diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 6c109675d..ad2ea05a2 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -19,78 +19,17 @@ #pragma once -#include +#include #include + +#include +#include + #include #include namespace gtsam { -// To play as a GenericValue, we need the following traits -namespace traits { - -// trait to wrap the default equals of types -template -struct equals { - typedef T type; - typedef bool result_type; - bool operator()(const T& a, const T& b, double tol) { - return a.equals(b, tol); - } -}; - -// trait to wrap the default print of types -template -struct print { - typedef T type; - typedef void result_type; - void operator()(const T& obj, const std::string& str) { - obj.print(str); - } -}; - -// equals for scalars -template<> -struct equals { - typedef double type; - typedef bool result_type; - bool operator()(double a, double b, double tol) { - return std::abs(a - b) <= tol; - } -}; - -// print for scalars -template<> -struct print { - typedef double type; - typedef void result_type; - void operator()(double a, const std::string& str) { - std::cout << str << ": " << a << std::endl; - } -}; - -// equals for Matrix types -template -struct equals > { - typedef Eigen::Matrix type; - typedef bool result_type; - bool operator()(const type& A, const type& B, double tol) { - return equal_with_abs_tol(A, B, tol); - } -}; - -// print for Matrix types -template -struct print > { - typedef Eigen::Matrix type; - typedef void result_type; - void operator()(const type& A, const std::string& str) { - std::cout << str << ": " << A << std::endl; - } -}; - -} - /** * Wraps any type T so it can play as a Value */ @@ -106,6 +45,8 @@ protected: T value_; ///< The wrapped value public: + // Only needed for serialization. + GenericValue(){} /// Construct from value GenericValue(const T& value) : @@ -131,31 +72,123 @@ public: // Cast the base class Value pointer to a templated generic class pointer const GenericValue& genericValue2 = static_cast(p); // Return the result of using the equals traits for the derived class - return traits::equals()(this->value_, genericValue2.value_, tol); + return traits::Equals(this->value_, genericValue2.value_, tol); } /// non virtual equals function, uses traits bool equals(const GenericValue &other, double tol = 1e-9) const { - return traits::equals()(this->value(), other.value(), tol); + return traits::Equals(this->value(), other.value(), tol); } /// Virtual print function, uses traits virtual void print(const std::string& str) const { - traits::print()(value_, str); + traits::Print(value_, str); } - // Serialization below: + /** + * Create a duplicate object returned as a pointer to the generic Value interface. + * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. + * The result must be deleted with Value::deallocate_, not with the 'delete' operator. + */ + virtual Value* clone_() const { + void *place = boost::singleton_pool::malloc(); + GenericValue* ptr = new (place) GenericValue(*this); // calls copy constructor to fill in + return ptr; + } - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); - ar & BOOST_SERIALIZATION_NVP(value_); - } + /** + * Destroy and deallocate this object, only if it was originally allocated using clone_(). + */ + virtual void deallocate_() const { + this->~GenericValue(); // Virtual destructor cleans up the derived object + boost::singleton_pool::free((void*) this); // Release memory from pool + } -protected: + /** + * Clone this value (normal clone on the heap, delete with 'delete' operator) + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(*this); + } - // Assignment operator for this class not needed since GenericValue is an abstract class + /// Generic Value interface version of retract + virtual Value* retract_(const Vector& delta) const { + // Call retract on the derived class using the retract trait function + const T retractResult = traits::Retract(GenericValue::value(), delta); + + // Create a Value pointer copy of the result + void* resultAsValuePlace = + boost::singleton_pool::malloc(); + Value* resultAsValue = new (resultAsValuePlace) GenericValue(retractResult); + + // Return the pointer to the Value base class + return resultAsValue; + } + + /// Generic Value interface version of localCoordinates + virtual Vector localCoordinates_(const Value& value2) const { + // Cast the base class Value pointer to a templated generic class pointer + const GenericValue& genericValue2 = + static_cast&>(value2); + + // Return the result of calling localCoordinates trait on the derived class + return traits::Local(GenericValue::value(), genericValue2.value()); + } + + /// Non-virtual version of retract + GenericValue retract(const Vector& delta) const { + return GenericValue(traits::Retract(GenericValue::value(), delta)); + } + + /// Non-virtual version of localCoordinates + Vector localCoordinates(const GenericValue& value2) const { + return localCoordinates_(value2); + } + + /// Return run-time dimensionality + virtual size_t dim() const { + return traits::GetDimension(value_); + } + + /// Assignment operator + virtual Value& operator=(const Value& rhs) { + // Cast the base class Value pointer to a derived class pointer + const GenericValue& derivedRhs = static_cast(rhs); + + // Do the assignment and return the result + *this = GenericValue(derivedRhs); // calls copy constructor + return *this; + } + + protected: + + // implicit assignment operator for (const GenericValue& rhs) works fine here + /// Assignment operator, protected because only the Value or DERIVED + /// assignment operators should be used. + // DerivedValue& operator=(const DerivedValue& rhs) { + // // Nothing to do, do not call base class assignment operator + // return *this; + // } + + private: + + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! + struct PoolTag { + }; + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("GenericValue", + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("value", value_); + } + +/// use this macro instead of BOOST_CLASS_EXPORT for GenericValues +#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(GenericValue) }; diff --git a/gtsam/base/Group.h b/gtsam/base/Group.h index 5b2574ee3..aec4b5f1c 100644 --- a/gtsam/base/Group.h +++ b/gtsam/base/Group.h @@ -19,43 +19,93 @@ #pragma once -#include +#include +#include +#include +#include namespace gtsam { +/// tag to assert a type is a group +struct group_tag {}; + +/// Group operator syntax flavors +struct multiplicative_group_tag {}; +struct additive_group_tag {}; + +template struct traits; + /** - * This concept check enforces a Group structure on a variable type, - * in which we require the existence of basic algebraic operations. + * Group Concept */ -template -class GroupConcept { -private: - static T concept_check(const T& t) { - /** assignment */ - T t2 = t; +template +class IsGroup { +public: + typedef typename traits::structure_category structure_category_tag; + typedef typename traits::group_flavor flavor_tag; + //typedef typename traits::identity::value_type identity_value_type; - /** identity */ - T identity = T::identity(); - - /** compose with another object */ - T compose_ret = identity.compose(t2); - - /** invert the object and yield a new one */ - T inverse_ret = compose_ret.inverse(); - - return inverse_ret; + BOOST_CONCEPT_USAGE(IsGroup) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a group (or derived)"); + e = traits::Identity(); + e = traits::Compose(g, h); + e = traits::Between(g, h); + e = traits::Inverse(g); + operator_usage(flavor); + // todo: how do we test the act concept? or do we even need to? } + +private: + void operator_usage(multiplicative_group_tag) { + e = g * h; + //e = -g; // todo this should work, but it is failing for Quaternions + } + void operator_usage(additive_group_tag) { + e = g + h; + e = h - g; + e = -g; + } + + flavor_tag flavor; + G e, g, h; + bool b; }; -} // \namespace gtsam +/// Check invariants +template +BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) // +check_group_invariants(const G& a, const G& b, double tol = 1e-9) { + G e = traits::Identity(); + return traits::Equals(traits::Compose(a, traits::Inverse(a)), e, tol) + && traits::Equals(traits::Between(a, b), traits::Compose(traits::Inverse(a), b), tol) + && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol); +} + +/// Macro to add group traits, additive flavor +#define GTSAM_ADDITIVE_GROUP(GROUP) \ + typedef additive_group_tag group_flavor; \ + static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \ + static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \ + static GROUP Inverse(const GROUP &g) { return -g;} + +/// Macro to add group traits, multiplicative flavor +#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \ + typedef additive_group_tag group_flavor; \ + static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \ + static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \ + static GROUP Inverse(const GROUP &g) { return g.inverse();} + +} // \ namespace gtsam /** - * Macros for using the GroupConcept + * Macros for using the IsGroup * - An instantiation for use inside unit tests * - A typedef for use inside generic algorithms * * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::GroupConcept; -#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; +#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup; +#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup _gtsam_IsGroup_##T; diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 7a9d32249..4a42bccf8 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -14,6 +14,9 @@ * @brief Base class and basic functions for Lie types * @author Richard Roberts * @author Alex Cunningham + * @author Frank Dellaert + * @author Mike Bosse + * @author Duy Nguyen Ta */ @@ -24,82 +27,245 @@ namespace gtsam { +/// A CRTP helper class that implements Lie group methods +/// Prerequisites: methods operator*, inverse, and AdjointMap, as well as a +/// ChartAtOrigin struct that will be used to define the manifold Chart +/// To use, simply derive, but also say "using LieGroup::inverse" +/// For derivative math, see doc/math.pdf +template +struct LieGroup { + + BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic, + "LieGroup not yet specialized for dynamically sized types."); + + enum { dimension = N }; + typedef OptionalJacobian ChartJacobian; + typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix TangentVector; + + const Class & derived() const { + return static_cast(*this); + } + + Class compose(const Class& g) const { + return derived() * g; + } + + Class between(const Class& g) const { + return derived().inverse() * g; + } + + Class compose(const Class& g, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + if (H1) *H1 = g.inverse().AdjointMap(); + if (H2) *H2 = Eigen::Matrix::Identity(); + return derived() * g; + } + + Class between(const Class& g, ChartJacobian H1, + ChartJacobian H2 = boost::none) const { + Class result = derived().inverse() * g; + if (H1) *H1 = - result.inverse().AdjointMap(); + if (H2) *H2 = Eigen::Matrix::Identity(); + return result; + } + + Class inverse(ChartJacobian H) const { + if (H) *H = - derived().AdjointMap(); + return derived().inverse(); + } + + Class expmap(const TangentVector& v) const { + return compose(Class::Expmap(v)); + } + + TangentVector logmap(const Class& g) const { + return Class::Logmap(between(g)); + } + + static Class Retract(const TangentVector& v) { + return Class::ChartAtOrigin::Retract(v); + } + + static TangentVector LocalCoordinates(const Class& g) { + return Class::ChartAtOrigin::Local(g); + } + + static Class Retract(const TangentVector& v, ChartJacobian H) { + return Class::ChartAtOrigin::Retract(v,H); + } + + static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) { + return Class::ChartAtOrigin::Local(g,H); + } + + Class retract(const TangentVector& v) const { + return compose(Class::ChartAtOrigin::Retract(v)); + } + + TangentVector localCoordinates(const Class& g) const { + return Class::ChartAtOrigin::Local(between(g)); + } + + Class retract(const TangentVector& v, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Jacobian D_g_v; + Class g = Class::ChartAtOrigin::Retract(v,D_g_v); + Class h = compose(g,H1,H2); + if (H2) *H2 = (*H2) * D_g_v; + return h; + } + + TangentVector localCoordinates(const Class& g, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Class h = between(g,H1,H2); + Jacobian D_v_h; + TangentVector v = Class::ChartAtOrigin::Local(h, D_v_h); + if (H1) *H1 = D_v_h * (*H1); + if (H2) *H2 = D_v_h * (*H2); + return v; + } + +}; + +/// tag to assert a type is a Lie group +struct lie_group_tag: public manifold_tag, public group_tag {}; + +namespace internal { + +/// A helper class that implements the traits interface for GTSAM lie groups. +/// To use this for your gtsam type, define: +/// template<> struct traits : public internal::LieGroupTraits {}; +template +struct LieGroupTraits : Testable { + typedef lie_group_tag structure_category; + + /// @name Group + /// @{ + typedef multiplicative_group_tag group_flavor; + static Class Identity() { return Class::identity();} + static Class Compose(const Class& m1, const Class& m2) { return m1 * m2;} + static Class Between(const Class& m1, const Class& m2) { return m1.inverse() * m2;} + static Class Inverse(const Class& m) { return m.inverse();} + /// @} + + /// @name Manifold + /// @{ + typedef Class ManifoldType; + enum { dimension = Class::dimension }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + + BOOST_STATIC_ASSERT_MSG(dimension != Eigen::Dynamic, + "LieGroupTraits not yet specialized for dynamically sized types."); + + static int GetDimension(const Class&) {return dimension;} + + static TangentVector Local(const Class& origin, const Class& other) { + return origin.localCoordinates(other); + } + + static Class Retract(const Class& origin, const TangentVector& v) { + return origin.retract(v); + } + + static TangentVector Local(const Class& origin, const Class& other, + ChartJacobian Horigin, ChartJacobian Hother = boost::none) { + return origin.localCoordinates(other, Horigin, Hother); + } + + static Class Retract(const Class& origin, const TangentVector& v, + ChartJacobian Horigin, ChartJacobian Hv = boost::none) { + return origin.retract(v, Horigin, Hv); + } + + /// @} + + /// @name Lie Group + /// @{ + + static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + return Class::Logmap(m, Hm); + } + + static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + return Class::Expmap(v, Hv); + } + + static Class Compose(const Class& m1, const Class& m2, ChartJacobian H1, + ChartJacobian H2 = boost::none) { + return m1.compose(m2, H1, H2); + } + + static Class Between(const Class& m1, const Class& m2, ChartJacobian H1, + ChartJacobian H2 = boost::none) { + return m1.between(m2, H1, H2); + } + + static Class Inverse(const Class& m, ChartJacobian H) { + return m.inverse(H); + } + + /// @} + +}; + +} // \ namepsace internal + /** * These core global functions can be specialized by new Lie types * for better performance. */ /** Compute l0 s.t. l2=l1*l0 */ -template -inline T between_default(const T& l1, const T& l2) {return l1.inverse().compose(l2);} +template +inline Class between_default(const Class& l1, const Class& l2) { + return l1.inverse().compose(l2); +} /** Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp */ -template -inline Vector logmap_default(const T& l0, const T& lp) {return T::Logmap(l0.between(lp));} +template +inline Vector logmap_default(const Class& l0, const Class& lp) { + return Class::Logmap(l0.between(lp)); +} /** Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */ -template -inline T expmap_default(const T& t, const Vector& d) {return t.compose(T::Expmap(d));} +template +inline Class expmap_default(const Class& t, const Vector& d) { + return t.compose(Class::Expmap(d)); +} /** - * Concept check class for Lie group type - * - * This concept check provides a specialization on the Manifold type, - * in which the Manifolds represented require an algebra and group structure. - * All Lie types must also be a Manifold. - * - * The necessary functions to implement for Lie are defined - * below with additional details as to the interface. The - * concept checking function in class Lie will check whether or not - * the function exists and throw compile-time errors. - * - * The exponential map is a specific retraction for Lie groups, - * which maps the tangent space in canonical coordinates back to - * the underlying manifold. Because we can enforce a group structure, - * a retraction of delta v, with tangent space centered at x1 can be performed - * as x2 = x1.compose(Expmap(v)). - * - * Expmap around identity - * static T Expmap(const Vector& v); - * - * Logmap around identity - * static Vector Logmap(const T& p); - * - * Compute l0 s.t. l2=l1*l0, where (*this) is l1 - * A default implementation of between(*this, lp) is available: - * between_default() - * T between(const T& l2) const; - * - * By convention, we use capital letters to designate a static function - * - * @tparam T is a Lie type, like Point2, Pose3, etc. + * Lie Group Concept */ -template -class LieConcept { -private: - /** concept checking function - implement the functions this demands */ - static T concept_check(const T& t) { +template +class IsLieGroup: public IsGroup, public IsManifold { +public: + typedef typename traits::structure_category structure_category_tag; + typedef typename traits::ManifoldType ManifoldType; + typedef typename traits::TangentVector TangentVector; + typedef typename traits::ChartJacobian ChartJacobian; - /** assignment */ - T t2 = t; + BOOST_CONCEPT_USAGE(IsLieGroup) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it is a Lie group (or derived)"); - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); - - /** expmap around identity */ - T expmap_identity_ret = T::Expmap(gtsam::zero(dim_ret)); - - /** Logmap around identity */ - Vector logmap_identity_ret = T::Logmap(t); - - /** Compute l0 s.t. l2=l1*l0, where (*this) is l1 */ - T between_ret = expmap_identity_ret.between(t2); - - return between_ret; + // group opertations with Jacobians + g = traits::Compose(g, h, Hg, Hh); + g = traits::Between(g, h, Hg, Hh); + g = traits::Inverse(g, Hg); + // log and exp map without Jacobians + g = traits::Expmap(v); + v = traits::Logmap(g); + // log and expnential map with Jacobians + g = traits::Expmap(v, Hg); + v = traits::Logmap(g, Hg); } - +private: + T g, h; + TangentVector v; + ChartJacobian Hg, Hh; }; /** @@ -146,12 +312,5 @@ T expm(const Vector& x, int K=7) { * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -#define GTSAM_CONCEPT_LIE_INST(T) \ - template class gtsam::ManifoldConcept; \ - template class gtsam::GroupConcept; \ - template class gtsam::LieConcept; - -#define GTSAM_CONCEPT_LIE_TYPE(T) \ - typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; \ - typedef gtsam::GroupConcept _gtsam_GroupConcept_##T; \ - typedef gtsam::LieConcept _gtsam_LieConcept_##T; +#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup; +#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup _gtsam_IsLieGroup_##T; diff --git a/gtsam/base/LieMatrix.h b/gtsam/base/LieMatrix.h index 7b120df1c..90b7207a2 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -25,15 +25,13 @@ #warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead." #endif -#include -#include -#include +#include #include namespace gtsam { /** - * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * @deprecated: LieMatrix, LieVector and LieMatrix are obsolete in GTSAM 4.0 as * we can directly add double, Vector, and Matrix into values now, because of * gtsam::traits. */ @@ -41,13 +39,17 @@ struct LieMatrix : public Matrix { /// @name Constructors /// @{ + enum { dimension = Eigen::Dynamic }; - /** default constructor - should be unnecessary */ + /** default constructor - only for serialize */ LieMatrix() {} /** initialize from a normal matrix */ LieMatrix(const Matrix& v) : Matrix(v) {} + template + LieMatrix(const M& v) : Matrix(v) {} + // Currently TMP constructor causes ICE on MSVS 2013 #if (_MSC_VER < 1800) /** initialize from a fixed size normal vector */ @@ -70,102 +72,57 @@ struct LieMatrix : public Matrix { inline bool equals(const LieMatrix& expected, double tol=1e-5) const { return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); } - + /// @} /// @name Standard Interface /// @{ - /** get the underlying vector */ + /** get the underlying matrix */ inline Matrix matrix() const { return static_cast(*this); } - + /// @} - /// @name Manifold interface + + /// @name Group + /// @{ + LieMatrix compose(const LieMatrix& q) { return (*this)+q;} + LieMatrix between(const LieMatrix& q) { return q-(*this);} + LieMatrix inverse() { return -(*this);} + /// @} + + /// @name Manifold + /// @{ + Vector localCoordinates(const LieMatrix& q) { return between(q).vector();} + LieMatrix retract(const Vector& v) {return compose(LieMatrix(v));} + /// @} + + /// @name Lie Group + /// @{ + static Vector Logmap(const LieMatrix& p) {return p.vector();} + static LieMatrix Expmap(const Vector& v) { return LieMatrix(v);} + /// @} + + /// @name VectorSpace requirements /// @{ /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return this->size(); } + inline size_t dim() const { return size(); } - /** Update the LieMatrix with a tangent space update. The elements of the - * tangent space vector correspond to the matrix entries arranged in - * *row-major* order. */ - inline LieMatrix retract(const Vector& v) const { - if(v.size() != this->size()) - throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size"); - - return LieMatrix(*this + - Eigen::Map >( - &v(0), this->rows(), this->cols())); - } - - /** @return the local coordinates of another object. The elements of the - * tangent space vector correspond to the matrix entries arranged in - * *row-major* order. */ - inline Vector localCoordinates(const LieMatrix& t2) const { - Vector result(this->size()); - Eigen::Map >( - &result(0), this->rows(), this->cols()) = t2 - *this; + /** Convert to vector, is done row-wise - TODO why? */ + inline Vector vector() const { + Vector result(size()); + typedef Eigen::Matrix RowMajor; + Eigen::Map(&result(0), rows(), cols()) = *this; return result; } - /// @} - /// @name Group interface - /// @{ - /** identity - NOTE: no known size at compile time - so zero length */ inline static LieMatrix identity() { throw std::runtime_error("LieMatrix::identity(): Don't use this function"); return LieMatrix(); } - - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - inline LieMatrix compose(const LieMatrix& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); - - return LieMatrix(*this + p); - } - - /** between operation */ - inline LieMatrix between(const LieMatrix& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieMatrix(l2 - *this); - } - - /** invert the object and yield a new one */ - inline LieMatrix inverse(boost::optional H=boost::none) const { - if(H) *H = -eye(dim()); - - return LieMatrix(-(*this)); - } - - /// @} - /// @name Lie group interface - /// @{ - - /** Expmap around identity */ - static inline LieMatrix Expmap(const Vector& v) { - throw std::runtime_error("LieMatrix::Expmap(): Don't use this function"); - return LieMatrix(v); } - - /** Logmap around identity */ - static inline Vector Logmap(const LieMatrix& p) { - Vector result(p.size()); - Eigen::Map >( - result.data(), p.rows(), p.cols()) = p; - return result; - } - /// @} private: @@ -181,17 +138,20 @@ private: }; -// Define GTSAM traits -namespace traits { template<> -struct is_manifold : public boost::true_type { -}; +struct traits : public internal::VectorSpace { -template<> -struct dimension : public Dynamic { -}; + // Override Retract, as the default version does not know how to initialize + static LieMatrix Retract(const LieMatrix& origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(origin); + if (H2) *H2 = Eye(origin); + typedef const Eigen::Matrix RowMajor; + return origin + Eigen::Map(&v(0), origin.rows(), origin.cols()); + } -} +}; } // \namespace gtsam diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index 4e40e03a9..9f6c56b28 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -24,8 +24,7 @@ #endif #include -#include -#include +#include namespace gtsam { @@ -36,11 +35,13 @@ namespace gtsam { */ struct GTSAM_EXPORT LieScalar { + enum { dimension = 1 }; + /** default constructor */ LieScalar() : d_(0.0) {} /** wrap a double */ - explicit LieScalar(double d) : d_(d) {} + /*explicit*/ LieScalar(double d) : d_(d) {} /** access the underlying value */ double value() const { return d_; } @@ -48,93 +49,43 @@ namespace gtsam { /** Automatic conversion to underlying value */ operator double() const { return d_; } - /** print @param name optional string naming the object */ - void print(const std::string& name="") const; + /** convert vector */ + Vector1 vector() const { Vector1 v; v< H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(1); - if(H2) *H2 = eye(1); - return LieScalar(d_ + p.d_); - } - - /** between operation */ - LieScalar between(const LieScalar& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(1); - if(H2) *H2 = eye(1); - return LieScalar(l2.value() - value()); - } - - /** invert the object and yield a new one */ - LieScalar inverse() const { - return LieScalar(-1.0 * value()); - } - - // Lie functions - - /** Expmap around identity */ - static LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); } - - /** Logmap around identity - just returns with default cast back */ - static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); } - - /// Left-trivialized derivative of the exponential map - static Matrix ExpmapDerivative(const Vector& v) { - return eye(1); - } - - /// Left-trivialized derivative inverse of the exponential map - static Matrix LogmapDerivative(const Vector& v) { - return eye(1); - } + /// @name Lie Group + /// @{ + static Vector1 Logmap(const LieScalar& p) { return p.vector();} + static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);} + /// @} private: double d_; }; - // Define GTSAM traits - namespace traits { - template<> - struct is_group : public boost::true_type { - }; - - template<> - struct is_manifold : public boost::true_type { - }; - - template<> - struct dimension : public boost::integral_constant { - }; - - } + struct traits : public internal::ScalarTraits {}; } // \namespace gtsam diff --git a/gtsam/base/LieVector.cpp b/gtsam/base/LieVector.cpp index f6cae28e2..84afdabc8 100644 --- a/gtsam/base/LieVector.cpp +++ b/gtsam/base/LieVector.cpp @@ -35,4 +35,6 @@ void LieVector::print(const std::string& name) const { gtsam::print(vector(), name); } +// Does not compile because LieVector is not fixed size. +// GTSAM_CONCEPT_LIE_INST(LieVector) } // \namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index d158548ad..c2003df3c 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -23,25 +23,28 @@ #warning "LieVector.h is deprecated. Please use Eigen::Vector instead." #endif -#include -#include -#include +#include namespace gtsam { /** - * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as + * @deprecated: LieVector, LieVector and LieMatrix are obsolete in GTSAM 4.0 as * we can directly add double, Vector, and Matrix into values now, because of * gtsam::traits. */ struct LieVector : public Vector { + enum { dimension = Eigen::Dynamic }; + /** default constructor - should be unnecessary */ LieVector() {} /** initialize from a normal vector */ LieVector(const Vector& v) : Vector(v) {} + template + LieVector(const V& v) : Vector(v) {} + // Currently TMP constructor causes ICE on MSVS 2013 #if (_MSC_VER < 1800) /** initialize from a fixed size normal vector */ @@ -55,75 +58,51 @@ struct LieVector : public Vector { /** constructor with size and initial data, row order ! */ GTSAM_EXPORT LieVector(size_t m, const double* const data); + /// @name Testable + /// @{ + GTSAM_EXPORT void print(const std::string& name="") const; + bool equals(const LieVector& expected, double tol=1e-5) const { + return gtsam::equal(vector(), expected.vector(), tol); + } + /// @} + + /// @name Group + /// @{ + LieVector compose(const LieVector& q) { return (*this)+q;} + LieVector between(const LieVector& q) { return q-(*this);} + LieVector inverse() { return -(*this);} + /// @} + + /// @name Manifold + /// @{ + Vector localCoordinates(const LieVector& q) { return between(q).vector();} + LieVector retract(const Vector& v) {return compose(LieVector(v));} + /// @} + + /// @name Lie Group + /// @{ + static Vector Logmap(const LieVector& p) {return p.vector();} + static LieVector Expmap(const Vector& v) { return LieVector(v);} + /// @} + + /// @name VectorSpace requirements + /// @{ + /** get the underlying vector */ Vector vector() const { return static_cast(*this); } - /** print @param name optional string naming the object */ - GTSAM_EXPORT void print(const std::string& name="") const; - - /** equality up to tolerance */ - bool equals(const LieVector& expected, double tol=1e-5) const { - return gtsam::equal(vector(), expected.vector(), tol); - } - - // Manifold requirements - /** Returns dimensionality of the tangent space */ size_t dim() const { return this->size(); } - /** Update the LieVector with a tangent space update */ - LieVector retract(const Vector& v) const { return LieVector(vector() + v); } - - /** @return the local coordinates of another object */ - Vector localCoordinates(const LieVector& t2) const { return LieVector(t2 - vector()); } - - // Group requirements - /** identity - NOTE: no known size at compile time - so zero length */ static LieVector identity() { throw std::runtime_error("LieVector::identity(): Don't use this function"); return LieVector(); } - // Note: Manually specifying the 'gtsam' namespace for the optional Matrix arguments - // This is a work-around for linux g++ 4.6.1 that incorrectly selects the Eigen::Matrix class - // instead of the gtsam::Matrix class. This is related to deriving this class from an Eigen Vector - // as the other geometry objects (Point3, Rot3, etc.) have this problem - /** compose with another object */ - LieVector compose(const LieVector& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(dim()); - if(H2) *H2 = eye(p.dim()); - - return LieVector(vector() + p); - } - - /** between operation */ - LieVector between(const LieVector& l2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(dim()); - if(H2) *H2 = eye(l2.dim()); - return LieVector(l2.vector() - vector()); - } - - /** invert the object and yield a new one */ - LieVector inverse(boost::optional H=boost::none) const { - if(H) *H = -eye(dim()); - - return LieVector(-1.0 * vector()); - } - - // Lie functions - - /** Expmap around identity */ - static LieVector Expmap(const Vector& v) { return LieVector(v); } - - /** Logmap around identity - just returns with default cast back */ - static Vector Logmap(const LieVector& p) { return p; } + /// @} private: @@ -136,17 +115,8 @@ private: } }; -// Define GTSAM traits -namespace traits { template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public Dynamic { -}; - -} +struct traits : public internal::VectorSpace {}; } // \namespace gtsam diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 1190822ed..b07bd3575 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -14,17 +14,24 @@ * @brief Base class and basic functions for Manifold types * @author Alex Cunningham * @author Frank Dellaert + * @author Mike Bosse */ #pragma once #include -#include -#include -#include +#include +#include + +#include +#include +#include namespace gtsam { +/// tag to assert a type is a manifold +struct manifold_tag {}; + /** * A manifold defines a space in which there is a notion of a linear tangent space * that can be centered around a given point on the manifold. These nonlinear @@ -43,325 +50,130 @@ namespace gtsam { * */ -// Traits, same style as Boost.TypeTraits -// All meta-functions below ever only declare a single type -// or a type/value/value_type -namespace traits { +template struct traits; -// is group, by default this is false -template -struct is_group: public boost::false_type { -}; +namespace internal { -// identity, no default provided, by default given by default constructor -template -struct identity { - static T value() { - return T(); +/// Requirements on type to pass it to Manifold template below +template +struct HasManifoldPrereqs { + + enum { dim = Class::dimension }; + + Class p, q; + Eigen::Matrix v; + OptionalJacobian Hp, Hq, Hv; + + BOOST_CONCEPT_USAGE(HasManifoldPrereqs) { + v = p.localCoordinates(q); + q = p.retract(v); } }; -// is manifold, by default this is false -template -struct is_manifold: public boost::false_type { -}; - -// dimension, can return Eigen::Dynamic (-1) if not known at compile time -// defaults to dynamic, TODO makes sense ? -typedef boost::integral_constant Dynamic; -template -struct dimension: public Dynamic { -}; - -/** - * zero::value is intended to be the origin of a canonical coordinate system - * with canonical(t) == DefaultChart::local(zero::value, t) - * Below we provide the group identity as zero *in case* it is a group - */ -template struct zero: public identity { - BOOST_STATIC_ASSERT(is_group::value); -}; - -// double - -template<> -struct is_group : public boost::true_type { -}; - -template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public boost::integral_constant { -}; - -template<> -struct zero { - static double value() { - return 0; +/// Extra manifold traits for fixed-dimension types +template +struct ManifoldImpl { + // Compile-time dimensionality + static int GetDimension(const Class&) { + return N; } }; -// Fixed size Eigen::Matrix type - -template -struct is_group > : public boost::true_type { -}; - -template -struct is_manifold > : public boost::true_type { -}; - -template -struct dimension > : public boost::integral_constant< - int, - M == Eigen::Dynamic ? Eigen::Dynamic : - (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> { - //TODO after switch to c++11 : the above should should be extracted to a constexpr function - // for readability and to reduce code duplication -}; - -template -struct zero > { - BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), - "traits::zero is only supported for fixed-size matrices"); - static Eigen::Matrix value() { - return Eigen::Matrix::Zero(); +/// Extra manifold traits for variable-dimension types +template +struct ManifoldImpl { + // Run-time dimensionality + static int GetDimension(const Class& m) { + return m.dim(); } }; -template -struct identity > : public zero< - Eigen::Matrix > { -}; +/// A helper that implements the traits interface for GTSAM manifolds. +/// To use this for your class type, define: +/// template<> struct traits : public Manifold { }; +template +struct Manifold: Testable, ManifoldImpl { -template struct is_chart: public boost::false_type { -}; + // Check that Class has the necessary machinery + BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); -} // \ namespace traits + // Dimension of the manifold + enum { dimension = Class::dimension }; -// Chart is a map from T -> vector, retract is its inverse -template -struct DefaultChart { - //BOOST_STATIC_ASSERT(traits::is_manifold::value); - typedef T type; - typedef Eigen::Matrix::value, 1> vector; + // Typedefs required by all manifold types. + typedef Class ManifoldType; + typedef manifold_tag structure_category; + typedef Eigen::Matrix TangentVector; - static vector local(const T& origin, const T& other) { + // Local coordinates + static TangentVector Local(const Class& origin, const Class& other) { return origin.localCoordinates(other); } - static T retract(const T& origin, const vector& d) { - return origin.retract(d); - } - static int getDimension(const T& origin) { - return origin.dim(); + + // Retraction back to manifold + static Class Retract(const Class& origin, const TangentVector& v) { + return origin.retract(v); } }; -namespace traits { -// populate default traits -template struct is_chart > : public boost::true_type { -}; -template struct dimension > : public dimension { -}; +} // \ namespace internal + +/// Check invariants for Manifold type +template +BOOST_CONCEPT_REQUIRES(((IsTestable)),(bool)) // +check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { + typename traits::TangentVector v0 = traits::Local(a,a); + typename traits::TangentVector v = traits::Local(a,b); + T c = traits::Retract(a,v); + return v0.norm() < tol && traits::Equals(b,c,tol); } -template -struct ChartConcept { +/// Manifold concept +template +class IsManifold { + public: - typedef typename C::type type; - typedef typename C::vector vector; - BOOST_CONCEPT_USAGE(ChartConcept) { - // is_chart trait should be true - BOOST_STATIC_ASSERT((traits::is_chart::value)); + typedef typename traits::structure_category structure_category_tag; + static const size_t dim = traits::dimension; + typedef typename traits::ManifoldType ManifoldType; + typedef typename traits::TangentVector TangentVector; - /** - * Returns Retraction update of val_ - */ - type retract_ret = C::retract(val_, vec_); + BOOST_CONCEPT_USAGE(IsManifold) { + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a manifold (or derived)"); + BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - /** - * Returns local coordinates of another object - */ - vec_ = C::local(val_, retract_ret); - - // a way to get the dimension that is compatible with dynamically sized types - dim_ = C::getDimension(val_); + // make sure Chart methods are defined + v = traits::Local(p, q); + q = traits::Retract(p, v); } private: - type val_; - vector vec_; - int dim_; + + TangentVector v; + ManifoldType p, q; }; -/** - * CanonicalChart > is a chart around zero::value - * Canonical is CanonicalChart > - * An example is Canonical - */ -template struct CanonicalChart { - BOOST_CONCEPT_ASSERT((ChartConcept)); - - typedef C Chart; - typedef typename Chart::type type; - typedef typename Chart::vector vector; - - // Convert t of type T into canonical coordinates - vector local(const type& t) { - return Chart::local(traits::zero::value(), t); - } - // Convert back from canonical coordinates to T - type retract(const vector& v) { - return Chart::retract(traits::zero::value(), v); - } -}; -template struct Canonical: public CanonicalChart > { -}; - -// double - -template<> -struct DefaultChart { - typedef double type; - typedef Eigen::Matrix vector; - - static vector local(double origin, double other) { - vector d; - d << other - origin; - return d; - } - static double retract(double origin, const vector& d) { - return origin + d[0]; - } - static int getDimension(double) { - return 1; - } -}; - -// Fixed size Eigen::Matrix type - -template -struct DefaultChart > { - /** - * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options). - * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size. - */ - typedef Eigen::Matrix type; - typedef type T; - typedef Eigen::Matrix::value, 1> vector; - - BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), - "Internal error: DefaultChart for Dynamic should be handled by template below"); - - static vector local(const T& origin, const T& other) { - return reshape(other) - - reshape(origin); - } - static T retract(const T& origin, const vector& d) { - return origin + reshape(d); - } - static int getDimension(const T&origin) { - return origin.rows() * origin.cols(); - } -}; - -// Dynamically sized Vector -template<> -struct DefaultChart { - typedef Vector type; - typedef Vector vector; - static vector local(const Vector& origin, const Vector& other) { - return other - origin; - } - static Vector retract(const Vector& origin, const vector& d) { - return origin + d; - } - static int getDimension(const Vector& origin) { - return origin.size(); - } -}; - -// Dynamically sized Matrix -template<> -struct DefaultChart { - typedef Matrix type; - typedef Vector vector; - static vector local(const Matrix& origin, const Matrix& other) { - Matrix d = other - origin; - return Eigen::Map(d.data(),getDimension(d)); - } - static Matrix retract(const Matrix& origin, const vector& d) { - return origin + Eigen::Map(d.data(),origin.rows(),origin.cols()); - } - static int getDimension(const Matrix& m) { - return m.size(); - } -}; - -/** - * Old Concept check class for Manifold types - * Requires a mapping between a linear tangent space and the underlying - * manifold, of which Lie is a specialization. - * - * The necessary functions to implement for Manifold are defined - * below with additional details as to the interface. The - * concept checking function in class Manifold will check whether or not - * the function exists and throw compile-time errors. - * - * Returns dimensionality of the tangent space, which may be smaller than the number - * of nonlinear parameters. - * size_t dim() const; - * - * Returns a new T that is a result of updating *this with the delta v after pulling - * the updated value back to the manifold T. - * T retract(const Vector& v) const; - * - * Returns the linear coordinates of lp in the tangent space centered around *this. - * Vector localCoordinates(const T& lp) const; - * - * By convention, we use capital letters to designate a static function - * @tparam T is a Lie type, like Point2, Pose3, etc. - */ -template -class ManifoldConcept { -private: - /** concept checking function - implement the functions this demands */ - static T concept_check(const T& t) { - - /** assignment */ - T t2 = t; - - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); - - /** - * Returns Retraction update of T - */ - T retract_ret = t.retract(gtsam::zero(dim_ret)); - - /** - * Returns local coordinates of another object - */ - Vector localCoords_ret = t.localCoordinates(t2); - - return retract_ret; - } +/// Give fixed size dimension of a type, fails at compile time if dynamic +template +struct FixedDimension { + typedef const int value_type; + static const int value = traits::dimension; + BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic, + "FixedDimension instantiated for dymanically-sized type."); }; } // \ namespace gtsam -/** - * Macros for using the ManifoldConcept - * - An instantiation for use inside unit tests - * - A typedef for use inside generic algorithms - * - * NOTE: intentionally not in the gtsam namespace to allow for classes not in - * the gtsam namespace to be more easily enforced as testable - */ -#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept; -#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept _gtsam_ManifoldConcept_##T; +///** +// * Macros for using the ManifoldConcept +// * - An instantiation for use inside unit tests +// * - A typedef for use inside generic algorithms +// * +// * NOTE: intentionally not in the gtsam namespace to allow for classes not in +// * the gtsam namespace to be more easily enforced as testable +// */ +#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold _gtsam_IsManifold_##T; diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 1469e265d..7bcd32b9f 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -238,11 +238,6 @@ istream& operator>>(istream& inputStream, Matrix& destinationMatrix) { return inputStream; } -/* ************************************************************************* */ -void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) { - fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; -} - /* ************************************************************************* */ Matrix diag(const std::vector& Hs) { size_t rows = 0, cols = 0; @@ -665,19 +660,6 @@ Matrix expm(const Matrix& A, size_t K) { return E; } -/* ************************************************************************* */ -Matrix Cayley(const Matrix& A) { - Matrix::Index n = A.cols(); - assert(A.rows() == n); - - // original -// const Matrix I = eye(n); -// return (I-A)*inverse(I+A); - - // inlined to let Eigen do more optimization - return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse(); -} - /* ************************************************************************* */ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal) { diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 3ecfcf8fa..20a4a6bc4 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -237,7 +237,10 @@ Eigen::Block sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, * @param i is the row of the upper left corner insert location * @param j is the column of the upper left corner insert location */ -GTSAM_EXPORT void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j); +template +GTSAM_EXPORT void insertSub(Eigen::MatrixBase& fullMatrix, const Eigen::MatrixBase& subMatrix, size_t i, size_t j) { + fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; +} /** * Create a matrix with submatrices along its diagonal @@ -525,17 +528,6 @@ DLT(const Matrix& A, double rank_tol = 1e-9); */ GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7); -/// Cayley transform -GTSAM_EXPORT Matrix Cayley(const Matrix& A); - -/// Implementation of Cayley transform using fixed size matrices to let -/// Eigen do more optimization -template -Eigen::Matrix CayleyFixed(const Eigen::Matrix& A) { - typedef Eigen::Matrix FMat; - return (FMat::Identity() - A)*(FMat::Identity() + A).inverse(); -} - std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); } // namespace gtsam @@ -568,4 +560,5 @@ namespace boost { } // namespace serialization } // namespace boost -BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix) +BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Matrix); + diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 5651816ba..08b8151dc 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -11,7 +11,7 @@ /** * @file OptionalJacobian.h - * @brief Special class for optional Matrix arguments + * @brief Special class for optional Jacobian arguments * @author Frank Dellaert * @author Natesh Srinivasan * @date Nov 28, 2014 @@ -33,23 +33,24 @@ namespace gtsam { * matrix will be resized. Finally, there is a constructor that takes * boost::none, the default constructor acts like boost::none, and * boost::optional is also supported for backwards compatibility. + * Below this class, a dynamic version is also implemented. */ template class OptionalJacobian { public: - /// Fixed size type - typedef Eigen::Matrix Fixed; + /// ::Jacobian size type + typedef Eigen::Matrix Jacobian; private: - Eigen::Map map_; /// View on constructor argument, if given + Eigen::Map map_; /// View on constructor argument, if given // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html // uses "placement new" to make map_ usurp the memory of the fixed size matrix void usurp(double* data) { - new (&map_) Eigen::Map(data); + new (&map_) Eigen::Map(data); } public: @@ -60,13 +61,13 @@ public: } /// Constructor that will usurp data of a fixed-size matrix - OptionalJacobian(Fixed& fixed) : + OptionalJacobian(Jacobian& fixed) : map_(NULL) { usurp(fixed.data()); } /// Constructor that will usurp data of a fixed-size matrix, pointer version - OptionalJacobian(Fixed* fixedPtr) : + OptionalJacobian(Jacobian* fixedPtr) : map_(NULL) { if (fixedPtr) usurp(fixedPtr->data()); @@ -103,12 +104,68 @@ public: } /// De-reference, like boost optional - Eigen::Map& operator*() { + Eigen::Map& operator*() { return map_; } /// TODO: operator->() - Eigen::Map* operator->(){ return &map_; } + Eigen::Map* operator->(){ return &map_; } +}; + +// The pure dynamic specialization of this is needed to support +// variable-sized types. Note that this is designed to work like the +// boost optional scheme from GTSAM 3. +template<> +class OptionalJacobian { + +public: + + /// Jacobian size type + typedef Eigen::MatrixXd Jacobian; + +private: + + Jacobian* pointer_; /// View on constructor argument, if given + +public: + + /// Default constructor acts like boost::none + OptionalJacobian() : + pointer_(NULL) { + } + + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd& dynamic) : + pointer_(&dynamic) { + } + +#ifndef OPTIONALJACOBIAN_NOBOOST + + /// Constructor with boost::none just makes empty + OptionalJacobian(boost::none_t none) : + pointer_(NULL) { + } + + /// Constructor compatible with old-style derivatives + OptionalJacobian(const boost::optional optional) : + pointer_(NULL) { + if (optional) pointer_ = &(*optional); + } + +#endif + + /// Return true is allocated, false if default constructor was used + operator bool() const { + return pointer_; + } + + /// De-reference, like boost optional + Jacobian& operator*() { + return *pointer_; + } + + /// TODO: operator->() + Jacobian* operator->(){ return pointer_; } }; } // namespace gtsam diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index a308c50a1..4790530ab 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -41,45 +42,52 @@ namespace gtsam { + // Forward declaration + template struct traits; + /** * A testable concept check that should be placed in applicable unit * tests and in generic algorithms. * * See macros for details on using this structure * @addtogroup base - * @tparam T is the type this constrains to be testable - assumes print() and equals() + * @tparam T is the objectype this constrains to be testable - assumes print() and equals() */ template - class TestableConcept { - static bool checkTestableConcept(const T& d) { + class IsTestable { + T t; + bool r1,r2; + public: + + BOOST_CONCEPT_USAGE(IsTestable) { // check print function, with optional string - d.print(std::string()); - d.print(); + traits::Print(t, std::string()); + traits::Print(t); // check print, with optional threshold double tol = 1.0; - bool r1 = d.equals(d, tol); - bool r2 = d.equals(d); - return r1 && r2; + r1 = traits::Equals(t,t,tol); + r2 = traits::Equals(t,t); } - }; + }; // \ Testable - /** Call print on the object */ - template - inline void print(const T& object, const std::string& s = "") { - object.print(s); + inline void print(float v, const std::string& s = "") { + printf("%s%f\n",s.c_str(),v); + } + inline void print(double v, const std::string& s = "") { + printf("%s%lf\n",s.c_str(),v); } /** Call equal on the object */ template inline bool equal(const T& obj1, const T& obj2, double tol) { - return obj1.equals(obj2, tol); + return traits::Equals(obj1,obj2, tol); } - /** Call equal on the object without tolerance (use default tolerance) */ + /** Call equal without tolerance (use default tolerance) */ template inline bool equal(const T& obj1, const T& obj2) { - return obj1.equals(obj2); + return traits::Equals(obj1,obj2); } /** @@ -87,11 +95,11 @@ namespace gtsam { */ template bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) { - if (actual.equals(expected, tol)) + if (traits::Equals(actual,expected, tol)) return true; printf("Not equal:\n"); - expected.print("expected:\n"); - actual.print("actual:\n"); + traits::Print(expected,"expected:\n"); + traits::Print(actual,"actual:\n"); return false; } @@ -103,7 +111,7 @@ namespace gtsam { double tol_; equals(double tol = 1e-9) : tol_(tol) {} bool operator()(const V& expected, const V& actual) { - return (actual.equals(expected, tol_)); + return (traits::Equals(actual, expected, tol_)); } }; @@ -116,7 +124,39 @@ namespace gtsam { equals_star(double tol = 1e-9) : tol_(tol) {} bool operator()(const boost::shared_ptr& expected, const boost::shared_ptr& actual) { if (!actual || !expected) return false; - return (actual->equals(*expected, tol_)); + return (traits::Equals(*actual,*expected, tol_)); + } + }; + + /// Requirements on type to pass it to Testable template below + template + struct HasTestablePrereqs { + + BOOST_CONCEPT_USAGE(HasTestablePrereqs) { + t->print(str); + b = t->equals(*s,tol); + } + + T *t, *s; // Pointer is to allow abstract classes + bool b; + double tol; + std::string str; + }; + + /// A helper that implements the traits interface for GTSAM types. + /// To use this for your gtsam type, define: + /// template<> struct traits : public Testable { }; + template + struct Testable { + + // Check that T has the necessary methods + BOOST_CONCEPT_ASSERT((HasTestablePrereqs)); + + static void Print(const T& m, const std::string& str = "") { + m.print(str); + } + static bool Equals(const T& m1, const T& m2, double tol = 1e-8) { + return m1.equals(m2, tol); } }; @@ -129,6 +169,7 @@ namespace gtsam { * * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable + * @deprecated please use BOOST_CONCEPT_ASSERT and */ -#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept _gtsam_TestableConcept_##T; +#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::IsTestable _gtsam_Testable_##T; diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h index afc244d6c..424fe5caf 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -168,7 +168,8 @@ namespace gtsam { * */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) {} + void serialize(ARCHIVE & ar, const unsigned int version) { + } }; diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h new file mode 100644 index 000000000..094e6f162 --- /dev/null +++ b/gtsam/base/VectorSpace.h @@ -0,0 +1,471 @@ +/* + * VectorSpace.h + * + * @date December 21, 2014 + * @author Mike Bosse + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/// tag to assert a type is a vector space +struct vector_space_tag: public lie_group_tag { +}; + +template struct traits; + +namespace internal { + +/// VectorSpace Implementation for Fixed sizes +template +struct VectorSpaceImpl { + + /// @name Group + /// @{ + static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} + static Class Between(const Class& v1, const Class& v2) { return v2-v1;} + static Class Inverse(const Class& m) { return -m;} + /// @} + + /// @name Manifold + /// @{ + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; + typedef Eigen::Matrix Jacobian; + static int GetDimension(const Class&) { return N;} + + static TangentVector Local(const Class& origin, const Class& other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = - Jacobian::Identity(); + if (H2) *H2 = Jacobian::Identity(); + Class v = other-origin; + return v.vector(); + } + + static Class Retract(const Class& origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Jacobian::Identity(); + if (H2) *H2 = Jacobian::Identity(); + return origin + (Class)v; + } + + /// @} + + /// @name Lie Group + /// @{ + + static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + if (Hm) *Hm = Jacobian::Identity(); + return m.vector(); + } + + static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + if (Hv) *Hv = Jacobian::Identity(); + return Class(v); + } + + static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, + ChartJacobian H2) { + if (H1) *H1 = Jacobian::Identity(); + if (H2) *H2 = Jacobian::Identity(); + return v1 + v2; + } + + static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, + ChartJacobian H2) { + if (H1) *H1 = - Jacobian::Identity(); + if (H2) *H2 = Jacobian::Identity(); + return v2 - v1; + } + + static Class Inverse(const Class& v, ChartJacobian H) { + if (H) *H = - Jacobian::Identity(); + return -v; + } + + /// @} +}; + +/// VectorSpace implementation for dynamic types. +template +struct VectorSpaceImpl { + + /// @name Group + /// @{ + static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} + static Class Between(const Class& v1, const Class& v2) { return v2-v1;} + static Class Inverse(const Class& m) { return -m;} + /// @} + + /// @name Manifold + /// @{ + typedef Eigen::VectorXd TangentVector; + typedef OptionalJacobian ChartJacobian; + static int GetDimension(const Class& m) { return m.dim();} + + static Eigen::MatrixXd Eye(const Class& m) { + int dim = GetDimension(m); + return Eigen::MatrixXd::Identity(dim, dim); + } + + static TangentVector Local(const Class& origin, const Class& other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = - Eye(origin); + if (H2) *H2 = Eye(other); + Class v = other-origin; + return v.vector(); + } + + static Class Retract(const Class& origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(origin); + if (H2) *H2 = Eye(origin); + return origin + Class(v); + } + + /// @} + + /// @name Lie Group + /// @{ + + static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + if (Hm) *Hm = Eye(m); + return m.vector(); + } + + static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + Class result(v); + if (Hv) + *Hv = Eye(v); + return result; + } + + static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1, + ChartJacobian H2) { + if (H1) *H1 = Eye(v1); + if (H2) *H2 = Eye(v2); + return v1 + v2; + } + + static Class Between(const Class& v1, const Class& v2, ChartJacobian H1, + ChartJacobian H2) { + if (H1) *H1 = - Eye(v1); + if (H2) *H2 = Eye(v2); + return v2 - v1; + } + + static Class Inverse(const Class& v, ChartJacobian H) { + if (H) *H = -Eye(v); + return -v; + } + + /// @} +}; + +/// A helper that implements the traits interface for GTSAM lie groups. +/// To use this for your gtsam type, define: +/// template<> struct traits : public VectorSpace { }; +template +struct VectorSpace: Testable, VectorSpaceImpl { + + typedef vector_space_tag structure_category; + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Class Identity() { return Class::identity();} + /// @} + + /// @name Manifold + /// @{ + enum { dimension = Class::dimension}; + typedef Class ManifoldType; + /// @} + +}; + +/// A helper that implements the traits interface for GTSAM lie groups. +/// To use this for your gtsam type, define: +/// template<> struct traits : public ScalarTraits { }; +template +struct ScalarTraits : VectorSpaceImpl { + + typedef vector_space_tag structure_category; + + /// @name Testable + /// @{ + static void Print(Scalar m, const std::string& str = "") { + gtsam::print(m, str); + } + static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) { + return std::abs(v1 - v2) < tol; + } + /// @} + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Scalar Identity() { return 0;} + /// @} + + /// @name Manifold + /// @{ + typedef Scalar ManifoldType; + enum { dimension = 1 }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian<1, 1> ChartJacobian; + + static TangentVector Local(Scalar origin, Scalar other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1)[0] = -1.0; + if (H2) (*H2)[0] = 1.0; + TangentVector result; + result(0) = other - origin; + return result; + } + + static Scalar Retract(Scalar origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1)[0] = 1.0; + if (H2) (*H2)[0] = 1.0; + return origin + v[0]; + } + /// @} + + /// @name Lie Group + /// @{ + static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) { + if (H) (*H)[0] = 1.0; + return Local(0, m); + } + + static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + if (H) (*H)[0] = 1.0; + return v[0]; + } + /// @} + +}; + +} // namespace internal + +/// double +template<> struct traits : public internal::ScalarTraits { +}; + +/// float +template<> struct traits : public internal::ScalarTraits { +}; + +// traits for any fixed double Eigen matrix +template +struct traits > : + internal::VectorSpaceImpl< + Eigen::Matrix, M * N> { + + typedef vector_space_tag structure_category; + typedef Eigen::Matrix Fixed; + + /// @name Testable + /// @{ + static void Print(const Fixed& m, const std::string& str = "") { + gtsam::print(Eigen::MatrixXd(m), str); + } + static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) { + return equal_with_abs_tol(v1, v2, tol); + } + /// @} + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Fixed Identity() { return Fixed::Zero();} + /// @} + + /// @name Manifold + /// @{ + enum { dimension = M*N}; + typedef Fixed ManifoldType; + typedef Eigen::Matrix TangentVector; + typedef Eigen::Matrix Jacobian; + typedef OptionalJacobian ChartJacobian; + + static TangentVector Local(Fixed origin, Fixed other, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1) = -Jacobian::Identity(); + if (H2) (*H2) = Jacobian::Identity(); + TangentVector result; + Eigen::Map(result.data()) = other - origin; + return result; + } + + static Fixed Retract(Fixed origin, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) (*H1) = Jacobian::Identity(); + if (H2) (*H2) = Jacobian::Identity(); + return origin + Eigen::Map(v.data()); + } + /// @} + + /// @name Lie Group + /// @{ + static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) { + if (H) *H = Jacobian::Identity(); + TangentVector result; + Eigen::Map(result.data()) = m; + return result; + } + + static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + Fixed m; + m.setZero(); + if (H) *H = Jacobian::Identity(); + return m + Eigen::Map(v.data()); + } + /// @} +}; + + +namespace internal { + +// traits for dynamic Eigen matrices +template +struct DynamicTraits { + + typedef vector_space_tag structure_category; + typedef Eigen::Matrix Dynamic; + + /// @name Testable + /// @{ + static void Print(const Dynamic& m, const std::string& str = "") { + gtsam::print(Eigen::MatrixXd(m), str); + } + static bool Equals(const Dynamic& v1, const Dynamic& v2, + double tol = 1e-8) { + return equal_with_abs_tol(v1, v2, tol); + } + /// @} + + /// @name Group + /// @{ + typedef additive_group_tag group_flavor; + static Dynamic Identity() { + throw std::runtime_error("Identity not defined for dynamic types"); + } + /// @} + + /// @name Manifold + /// @{ + enum { dimension = Eigen::Dynamic }; + typedef Eigen::VectorXd TangentVector; + typedef Eigen::MatrixXd Jacobian; + typedef OptionalJacobian ChartJacobian; + typedef Dynamic ManifoldType; + + static int GetDimension(const Dynamic& m) { + return m.rows() * m.cols(); + } + + static Jacobian Eye(const Dynamic& m) { + int dim = GetDimension(m); + return Eigen::Matrix::Identity(dim, dim); + } + + static TangentVector Local(const Dynamic& m, const Dynamic& other, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = -Eye(m); + if (H2) *H2 = Eye(m); + TangentVector v(GetDimension(m)); + Eigen::Map(v.data(), m.rows(), m.cols()) = other - m; + return v; + } + + static Dynamic Retract(const Dynamic& m, const TangentVector& v, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(m); + if (H2) *H2 = Eye(m); + return m + Eigen::Map(v.data(), m.rows(), m.cols()); + } + /// @} + + /// @name Lie Group + /// @{ + static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) { + if (H) *H = Eye(m); + TangentVector result(GetDimension(m)); + Eigen::Map(result.data(), m.cols(), m.rows()) = m; + return result; + } + + static Dynamic Expmap(const TangentVector& v, ChartJacobian H = boost::none) { + throw std::runtime_error("Expmap not defined for dynamic types"); + } + + static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) { + if (H) *H = -Eye(m); + return -m; + } + + static Dynamic Compose(const Dynamic& v1, const Dynamic& v2, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = Eye(v1); + if (H2) *H2 = Eye(v1); + return v1 + v2; + } + + static Dynamic Between(const Dynamic& v1, const Dynamic& v2, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + if (H1) *H1 = -Eye(v1); + if (H2) *H2 = Eye(v1); + return v2 - v1; + } + /// @} + +}; + +} // \ internal + +// traits for fully dynamic matrix +template +struct traits > : + public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> { +}; + +// traits for dynamic column vector +template +struct traits > : + public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> { +}; + +// traits for dynamic row vector +template +struct traits > : + public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> { +}; + +/// Vector Space concept +template +class IsVectorSpace: public IsLieGroup { +public: + + typedef typename traits::structure_category structure_category_tag; + + BOOST_CONCEPT_USAGE(IsVectorSpace) { + 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; + } + +private: + T p, q, r; +}; + +} // namespace gtsam + diff --git a/gtsam/base/chartTesting.h b/gtsam/base/chartTesting.h index b1b47f020..f63054a5b 100644 --- a/gtsam/base/chartTesting.h +++ b/gtsam/base/chartTesting.h @@ -31,6 +31,9 @@ template void testDefaultChart(TestResult& result_, const std::string& name_, const T& value) { + + GTSAM_CONCEPT_TESTABLE_TYPE(T); + typedef typename gtsam::DefaultChart Chart; typedef typename Chart::vector Vector; diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h new file mode 100644 index 000000000..da872d006 --- /dev/null +++ b/gtsam/base/concepts.h @@ -0,0 +1,32 @@ +/* + * concepts.h + * + * @date Dec 4, 2014 + * @author Mike Bosse + * @author Frank Dellaert + */ + +#pragma once + +// This is a helper to ease the transition to the new traits defined in this file. +// Uncomment this if you want all methods that are tagged as not implemented +// to cause compilation errors. +#ifdef COMPILE_ERROR_NOT_IMPLEMENTED + +#include +#define CONCEPT_NOT_IMPLEMENTED BOOST_STATIC_ASSERT_MSG(boost::false_type, \ +"This method is required by the new concepts framework but has not been implemented yet."); + +#else + +#include +#define CONCEPT_NOT_IMPLEMENTED \ + throw std::runtime_error("This method is required by the new concepts framework but has not been implemented yet."); + +#endif + +namespace gtsam { + +template struct traits; + +} diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index b2c163c2d..e5cfbdd3b 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -28,11 +28,10 @@ #pragma GCC diagnostic pop #endif -#include -#include #include #include #include +#include namespace gtsam { @@ -58,25 +57,32 @@ namespace gtsam { * http://www.boost.org/doc/libs/release/libs/bind/bind.html */ + +// a quick helper struct to get the appropriate fixed sized matrix from two value types +namespace internal { +template +struct FixedSizeMatrix { + typedef Eigen::Matrix::dimension, traits::dimension> type; +}; +} + /** * Numerically compute gradient of scalar function * Class X is the input argument * The class X needs to have dim, expmap, logmap */ template -Vector numericalGradient(boost::function h, const X& x, +typename internal::FixedSizeMatrix::type numericalGradient(boost::function h, const X& x, double delta = 1e-5) { double factor = 1.0 / (2.0 * delta); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits::dimension::value; + static const int N = traits::dimension; BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); - typedef DefaultChart ChartX; - typedef typename ChartX::vector TangentX; - // get chart at x - ChartX chartX; + typedef typename traits::TangentVector TangentX; // Prepare a tangent vector to perturb x with, only works for fixed size TangentX d; @@ -85,9 +91,9 @@ Vector numericalGradient(boost::function h, const X& x, Vector g = zero(N); // Can be fixed size for (int j = 0; j < N; j++) { d(j) = delta; - double hxplus = h(chartX.retract(x, d)); + double hxplus = h(traits::Retract(x, d)); d(j) = -delta; - double hxmin = h(chartX.retract(x, d)); + double hxmin = h(traits::Retract(x, d)); d(j) = 0; g(j) = (hxplus - hxmin) * factor; } @@ -104,35 +110,33 @@ Vector numericalGradient(boost::function h, const X& x, * Class X is the input argument * @return m*n Jacobian computed via central differencing */ + template // TODO Should compute fixed-size matrix -Matrix numericalDerivative11(boost::function h, const X& x, +typename internal::FixedSizeMatrix::type numericalDerivative11(boost::function h, const X& x, double delta = 1e-5) { - using namespace traits; - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + typedef typename internal::FixedSizeMatrix::type Matrix; + + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - typedef DefaultChart ChartY; - typedef typename ChartY::vector TangentY; + typedef traits TraitsY; + typedef typename TraitsY::TangentVector TangentY; - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); - static const int N = traits::dimension::value; + static const int N = traits::dimension; BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type."); - typedef DefaultChart ChartX; - typedef typename ChartX::vector TangentX; + typedef traits TraitsX; + typedef typename TraitsX::TangentVector TangentX; // get value at x, and corresponding chart Y hx = h(x); - ChartY chartY; // Bit of a hack for now to find number of rows - TangentY zeroY = chartY.local(hx, hx); + TangentY zeroY = TraitsY::Local(hx, hx); size_t m = zeroY.size(); - // get chart at x - ChartX chartX; - // Prepare a tangent vector to perturb x with, only works for fixed size TangentX dx; dx.setZero(); @@ -142,9 +146,9 @@ Matrix numericalDerivative11(boost::function h, const X& x, double factor = 1.0 / (2.0 * delta); for (int j = 0; j < N; j++) { dx(j) = delta; - TangentY dy1 = chartY.local(hx, h(chartX.retract(x, dx))); + TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = -delta; - TangentY dy2 = chartY.local(hx, h(chartX.retract(x, dx))); + TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx))); dx(j) = 0; H.col(j) << (dy1 - dy2) * factor; } @@ -153,7 +157,7 @@ Matrix numericalDerivative11(boost::function h, const X& x, /** use a raw C++ function pointer */ template -Matrix numericalDerivative11(Y (*h)(const X&), const X& x, +typename internal::FixedSizeMatrix::type numericalDerivative11(Y (*h)(const X&), const X& x, double delta = 1e-5) { return numericalDerivative11(boost::bind(h, _1), x, delta); } @@ -167,18 +171,18 @@ Matrix numericalDerivative11(Y (*h)(const X&), const X& x, * @return m*n Jacobian computed via central differencing */ template -Matrix numericalDerivative21(const boost::function& h, +typename internal::FixedSizeMatrix::type numericalDerivative21(const boost::function& h, const X1& x1, const X2& x2, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11(boost::bind(h, _1, x2), x1, delta); } /** use a raw C++ function pointer */ template -inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, +typename internal::FixedSizeMatrix::type numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalDerivative21(boost::bind(h, _1, _2), x1, x2, delta); } @@ -192,18 +196,18 @@ inline Matrix numericalDerivative21(Y (*h)(const X1&, const X2&), const X1& x1, * @return m*n Jacobian computed via central differencing */ template -Matrix numericalDerivative22(boost::function h, +typename internal::FixedSizeMatrix::type numericalDerivative22(boost::function h, const X1& x1, const X2& x2, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, - "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, - "Template argument X2 must be a manifold type."); +// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), +// "Template argument X1 must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X2 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, _1), x2, delta); } /** use a raw C++ function pointer */ template -inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, +typename internal::FixedSizeMatrix::type numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalDerivative22(boost::bind(h, _1, _2), x1, x2, delta); } @@ -219,18 +223,18 @@ inline Matrix numericalDerivative22(Y (*h)(const X1&, const X2&), const X1& x1, * All classes Y,X1,X2,X3 need dim, expmap, logmap */ template -Matrix numericalDerivative31( +typename internal::FixedSizeMatrix::type numericalDerivative31( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X1 must be a manifold type."); return numericalDerivative11(boost::bind(h, _1, x2, x3), x1, delta); } template -inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), +typename internal::FixedSizeMatrix::type numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalDerivative31(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); @@ -247,18 +251,18 @@ inline Matrix numericalDerivative31(Y (*h)(const X1&, const X2&, const X3&), * All classes Y,X1,X2,X3 need dim, expmap, logmap */ template -Matrix numericalDerivative32( +typename internal::FixedSizeMatrix::type numericalDerivative32( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X2 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, _1, x3), x2, delta); } template -inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalDerivative32(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); @@ -275,18 +279,18 @@ inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), * All classes Y,X1,X2,X3 need dim, expmap, logmap */ template -Matrix numericalDerivative33( +typename internal::FixedSizeMatrix::type numericalDerivative33( boost::function h, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument Y must be a manifold type."); - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X3 must be a manifold type."); return numericalDerivative11(boost::bind(h, x1, x2, _1), x3, delta); } template -inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalDerivative33(boost::bind(h, _1, _2, _3), x1, x2, x3, delta); @@ -301,19 +305,20 @@ inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), * @return n*n Hessian matrix computed via central differencing */ template -inline Matrix numericalHessian(boost::function f, const X& x, +inline typename internal::FixedSizeMatrix::type numericalHessian(boost::function f, const X& x, double delta = 1e-5) { - BOOST_STATIC_ASSERT_MSG(traits::is_manifold::value, + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), "Template argument X must be a manifold type."); + typedef Eigen::Matrix::dimension, 1> VectorD; typedef boost::function F; - typedef boost::function G; + typedef boost::function G; G ng = static_cast(numericalGradient ); - return numericalDerivative11(boost::bind(ng, f, _1, delta), x, + return numericalDerivative11(boost::bind(ng, f, _1, delta), x, delta); } template -inline Matrix numericalHessian(double (*f)(const X&), const X& x, double delta = +inline typename internal::FixedSizeMatrix::type numericalHessian(double (*f)(const X&), const X& x, double delta = 1e-5) { return numericalHessian(boost::function(f), x, delta); } @@ -327,6 +332,8 @@ class G_x1 { X1 x1_; double delta_; public: + typedef typename internal::FixedSizeMatrix::type Vector; + G_x1(const boost::function& f, const X1& x1, double delta) : f_(f), x1_(x1), delta_(delta) { @@ -337,9 +344,10 @@ public: }; template -inline Matrix numericalHessian212( +inline typename internal::FixedSizeMatrix::type numericalHessian212( boost::function f, const X1& x1, const X2& x2, double delta = 1e-5) { + typedef typename internal::FixedSizeMatrix::type Vector; G_x1 g_x1(f, x1, delta); return numericalDerivative11( boost::function( @@ -347,17 +355,19 @@ inline Matrix numericalHessian212( } template -inline Matrix numericalHessian212(double (*f)(const X1&, const X2&), +inline typename internal::FixedSizeMatrix::type numericalHessian212(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalHessian212(boost::function(f), x1, x2, delta); } template -inline Matrix numericalHessian211( +inline typename internal::FixedSizeMatrix::type numericalHessian211( boost::function f, const X1& x1, const X2& x2, double delta = 1e-5) { + typedef typename internal::FixedSizeMatrix::type Vector; + Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; boost::function f2(boost::bind(f, _1, x2)); @@ -368,17 +378,17 @@ inline Matrix numericalHessian211( } template -inline Matrix numericalHessian211(double (*f)(const X1&, const X2&), +inline typename internal::FixedSizeMatrix::type numericalHessian211(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalHessian211(boost::function(f), x1, x2, delta); } template -inline Matrix numericalHessian222( +inline typename internal::FixedSizeMatrix::type numericalHessian222( boost::function f, const X1& x1, const X2& x2, double delta = 1e-5) { - + typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; boost::function f2(boost::bind(f, x1, _1)); @@ -389,7 +399,7 @@ inline Matrix numericalHessian222( } template -inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), +inline typename internal::FixedSizeMatrix::type numericalHessian222(double (*f)(const X1&, const X2&), const X1& x1, const X2& x2, double delta = 1e-5) { return numericalHessian222(boost::function(f), x1, x2, delta); @@ -400,10 +410,10 @@ inline Matrix numericalHessian222(double (*f)(const X1&, const X2&), */ /* **************************************************************** */ template -inline Matrix numericalHessian311( +inline typename internal::FixedSizeMatrix::type numericalHessian311( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - + typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X1&, double) = &numericalGradient; boost::function f2(boost::bind(f, _1, x2, x3)); @@ -414,7 +424,7 @@ inline Matrix numericalHessian311( } template -inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian311(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian311( boost::function(f), x1, x2, x3, @@ -423,10 +433,10 @@ inline Matrix numericalHessian311(double (*f)(const X1&, const X2&, const X3&), /* **************************************************************** */ template -inline Matrix numericalHessian322( +inline typename internal::FixedSizeMatrix::type numericalHessian322( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - + typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X2&, double) = &numericalGradient; boost::function f2(boost::bind(f, x1, _1, x3)); @@ -437,7 +447,7 @@ inline Matrix numericalHessian322( } template -inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian322(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian322( boost::function(f), x1, x2, x3, @@ -446,10 +456,10 @@ inline Matrix numericalHessian322(double (*f)(const X1&, const X2&, const X3&), /* **************************************************************** */ template -inline Matrix numericalHessian333( +inline typename internal::FixedSizeMatrix::type numericalHessian333( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { - + typedef typename internal::FixedSizeMatrix::type Vector; Vector (*numGrad)(boost::function, const X3&, double) = &numericalGradient; boost::function f2(boost::bind(f, x1, x2, _1)); @@ -460,7 +470,7 @@ inline Matrix numericalHessian333( } template -inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian333(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian333( boost::function(f), x1, x2, x3, @@ -469,7 +479,7 @@ inline Matrix numericalHessian333(double (*f)(const X1&, const X2&, const X3&), /* **************************************************************** */ template -inline Matrix numericalHessian312( +inline typename internal::FixedSizeMatrix::type numericalHessian312( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( @@ -478,7 +488,7 @@ inline Matrix numericalHessian312( } template -inline Matrix numericalHessian313( +inline typename internal::FixedSizeMatrix::type numericalHessian313( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( @@ -487,7 +497,7 @@ inline Matrix numericalHessian313( } template -inline Matrix numericalHessian323( +inline typename internal::FixedSizeMatrix::type numericalHessian323( boost::function f, const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian212( @@ -497,7 +507,7 @@ inline Matrix numericalHessian323( /* **************************************************************** */ template -inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian312(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian312( boost::function(f), x1, x2, x3, @@ -505,7 +515,7 @@ inline Matrix numericalHessian312(double (*f)(const X1&, const X2&, const X3&), } template -inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian313(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian313( boost::function(f), x1, x2, x3, @@ -513,7 +523,7 @@ inline Matrix numericalHessian313(double (*f)(const X1&, const X2&, const X3&), } template -inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), +inline typename internal::FixedSizeMatrix::type numericalHessian323(double (*f)(const X1&, const X2&, const X3&), const X1& x1, const X2& x2, const X3& x3, double delta = 1e-5) { return numericalHessian323( boost::function(f), x1, x2, x3, diff --git a/gtsam/base/testLie.h b/gtsam/base/testLie.h new file mode 100644 index 000000000..4bbca85ca --- /dev/null +++ b/gtsam/base/testLie.h @@ -0,0 +1,80 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file chartTesting.h + * @brief + * @date November, 2014 + * @author Paul Furgale + */ + +#pragma once + +#include +#include + +#include +#include +#include + +namespace gtsam { + +// Do a comprehensive test of Lie Group derivatives +template +void testLieGroupDerivatives(TestResult& result_, const std::string& name_, + const G& t1, const G& t2) { + + Matrix H1, H2; + typedef traits T; + + // Inverse + EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); + EXPECT(assert_equal(numericalDerivative11(T::Inverse, t1),H1)); + + EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); + EXPECT(assert_equal(numericalDerivative11(T::Inverse, t2),H1)); + + // Compose + EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Compose, t1, t2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Compose, t1, t2), H2)); + + // Between + EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Between, t1, t2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Between, t1, t2), H2)); +} +// Do a comprehensive test of Lie Group Chart derivatives +template +void testChartDerivatives(TestResult& result_, const std::string& name_, + const G& t1, const G& t2) { + + Matrix H1, H2; + typedef traits T; + + // Retract + typename G::TangentVector w12 = T::Local(t1, t2); + EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Retract, t1, w12), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Retract, t1, w12), H2)); + + // Local + EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative21(T::Local, t1, t2), H1)); + EXPECT(assert_equal(numericalDerivative22(T::Local, t1, t2), H2)); +} +} // namespace gtsam + +#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \ + { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); } + +#define CHECK_CHART_DERIVATIVES(t1,t2) \ + { gtsam::testChartDerivatives(result_, name_, t1, t2); } diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 53dd6d4d5..7cc649e66 100644 --- a/gtsam/base/tests/testLieMatrix.cpp +++ b/gtsam/base/tests/testLieMatrix.cpp @@ -30,7 +30,7 @@ TEST( LieMatrix, construction ) { Matrix m = (Matrix(2,2) << 1.0,2.0, 3.0,4.0).finished(); LieMatrix lie1(m), lie2(m); - EXPECT(lie1.dim() == 4); + EXPECT(traits::GetDimension(m) == 4); EXPECT(assert_equal(m, lie1.matrix())); EXPECT(assert_equal(lie1, lie2)); } @@ -50,17 +50,17 @@ TEST(LieMatrix, retract) { Vector update = (Vector(4) << 3.0, 4.0, 6.0, 7.0).finished(); LieMatrix expected((Matrix(2,2) << 4.0, 6.0, 9.0, 11.0).finished()); - LieMatrix actual = init.retract(update); + LieMatrix actual = traits::Retract(init,update); EXPECT(assert_equal(expected, actual)); Vector expectedUpdate = update; - Vector actualUpdate = init.localCoordinates(actual); + Vector actualUpdate = traits::Local(init,actual); EXPECT(assert_equal(expectedUpdate, actualUpdate)); Vector expectedLogmap = (Vector(4) << 1, 2, 3, 4).finished(); - Vector actualLogmap = LieMatrix::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); + Vector actualLogmap = traits::Logmap(LieMatrix((Matrix(2,2) << 1.0, 2.0, 3.0, 4.0).finished())); EXPECT(assert_equal(expectedLogmap, actualLogmap)); } diff --git a/gtsam/base/tests/testLieScalar.cpp b/gtsam/base/tests/testLieScalar.cpp index 946a342fc..060087c2a 100644 --- a/gtsam/base/tests/testLieScalar.cpp +++ b/gtsam/base/tests/testLieScalar.cpp @@ -27,6 +27,20 @@ GTSAM_CONCEPT_LIE_INST(LieScalar) const double tol=1e-9; +//****************************************************************************** +TEST(LieScalar , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(LieScalar , Invariants) { + LieScalar lie1(2), lie2(3); + check_group_invariants(lie1, lie2); + check_manifold_invariants(lie1, lie2); +} + /* ************************************************************************* */ TEST( testLieScalar, construction ) { double d = 2.; @@ -34,7 +48,7 @@ TEST( testLieScalar, construction ) { EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol); EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol); - EXPECT(lie1.dim() == 1); + EXPECT(traits::dimension == 1); EXPECT(assert_equal(lie1, lie2)); } @@ -42,7 +56,8 @@ TEST( testLieScalar, construction ) { TEST( testLieScalar, localCoordinates ) { LieScalar lie1(1.), lie2(3.); - EXPECT(assert_equal((Vector)(Vector(1) << 2).finished(), lie1.localCoordinates(lie2))); + Vector1 actual = traits::Local(lie1, lie2); + EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual)); } /* ************************************************************************* */ diff --git a/gtsam/base/tests/testLieVector.cpp b/gtsam/base/tests/testLieVector.cpp index c1dface2e..81e03c63c 100644 --- a/gtsam/base/tests/testLieVector.cpp +++ b/gtsam/base/tests/testLieVector.cpp @@ -25,7 +25,21 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(LieVector) GTSAM_CONCEPT_LIE_INST(LieVector) -/* ************************************************************************* */ +//****************************************************************************** +TEST(LieVector , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(LieVector , Invariants) { + Vector v = Vector3(1.0, 2.0, 3.0); + LieVector lie1(v), lie2(v); + check_manifold_invariants(lie1, lie2); +} + +//****************************************************************************** TEST( testLieVector, construction ) { Vector v = Vector3(1.0, 2.0, 3.0); LieVector lie1(v), lie2(v); @@ -35,17 +49,19 @@ TEST( testLieVector, construction ) { EXPECT(assert_equal(lie1, lie2)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testLieVector, other_constructors ) { Vector init = Vector2(10.0, 20.0); LieVector exp(init); - double data[] = {10,20}; - LieVector b(2,data); + double data[] = { 10, 20 }; + LieVector b(2, data); EXPECT(assert_equal(exp, b)); } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp index 5b36d2b02..13d32794e 100644 --- a/gtsam/base/tests/testMatrix.cpp +++ b/gtsam/base/tests/testMatrix.cpp @@ -16,12 +16,14 @@ * @author Carlos Nieto **/ -#include -#include +#include +#include +#include #include #include #include -#include +#include +#include using namespace std; using namespace gtsam; @@ -30,7 +32,7 @@ static double inf = std::numeric_limits::infinity(); static const double tol = 1e-9; /* ************************************************************************* */ -TEST( matrix, constructor_data ) +TEST(Matrix, constructor_data ) { Matrix A = (Matrix(2, 2) << -5, 3, 0, -5).finished(); @@ -44,7 +46,7 @@ TEST( matrix, constructor_data ) } /* ************************************************************************* */ -TEST( matrix, Matrix_ ) +TEST(Matrix, Matrix_ ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 0.0, -5.0).finished(); Matrix B(2, 2); @@ -74,7 +76,7 @@ namespace { } /* ************************************************************************* */ -TEST( matrix, special_comma_initializer) +TEST(Matrix, special_comma_initializer) { Matrix expected(2,2); expected(0,0) = 1; @@ -103,7 +105,7 @@ TEST( matrix, special_comma_initializer) } /* ************************************************************************* */ -TEST( matrix, col_major ) +TEST(Matrix, col_major ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); const double * const a = &A(0, 0); @@ -114,7 +116,7 @@ TEST( matrix, col_major ) } /* ************************************************************************* */ -TEST( matrix, collect1 ) +TEST(Matrix, collect1 ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); @@ -131,7 +133,7 @@ TEST( matrix, collect1 ) } /* ************************************************************************* */ -TEST( matrix, collect2 ) +TEST(Matrix, collect2 ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix B = (Matrix(2, 3) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); @@ -151,7 +153,7 @@ TEST( matrix, collect2 ) } /* ************************************************************************* */ -TEST( matrix, collect3 ) +TEST(Matrix, collect3 ) { Matrix A, B; A = eye(2, 3); @@ -168,7 +170,7 @@ TEST( matrix, collect3 ) } /* ************************************************************************* */ -TEST( matrix, stack ) +TEST(Matrix, stack ) { Matrix A = (Matrix(2, 2) << -5.0, 3.0, 00.0, -5.0).finished(); Matrix B = (Matrix(3, 2) << -0.5, 2.1, 1.1, 3.4, 2.6, 7.1).finished(); @@ -191,7 +193,7 @@ TEST( matrix, stack ) } /* ************************************************************************* */ -TEST( matrix, column ) +TEST(Matrix, column ) { Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., @@ -210,7 +212,7 @@ TEST( matrix, column ) } /* ************************************************************************* */ -TEST( matrix, insert_column ) +TEST(Matrix, insert_column ) { Matrix big = zeros(5, 6); Vector col = ones(5); @@ -229,7 +231,7 @@ TEST( matrix, insert_column ) } /* ************************************************************************* */ -TEST( matrix, insert_subcolumn ) +TEST(Matrix, insert_subcolumn ) { Matrix big = zeros(5, 6); Vector col1 = ones(2); @@ -252,7 +254,7 @@ TEST( matrix, insert_subcolumn ) } /* ************************************************************************* */ -TEST( matrix, row ) +TEST(Matrix, row ) { Matrix A = (Matrix(4, 7) << -1., 0., 1., 0., 0., 0., -0.2, 0., -1., 0., 1., 0., 0., 0.3, 1., 0., 0., 0., -1., 0., 0.2, 0., 1., 0., 0., 0., -1., @@ -271,7 +273,7 @@ TEST( matrix, row ) } /* ************************************************************************* */ -TEST( matrix, zeros ) +TEST(Matrix, zeros ) { Matrix A(2, 3); A(0, 0) = 0; @@ -287,7 +289,7 @@ TEST( matrix, zeros ) } /* ************************************************************************* */ -TEST( matrix, insert_sub ) +TEST(Matrix, insert_sub ) { Matrix big = zeros(5, 6), small = (Matrix(2, 3) << 1.0, 1.0, 1.0, 1.0, 1.0, 1.0).finished(); @@ -302,7 +304,7 @@ TEST( matrix, insert_sub ) } /* ************************************************************************* */ -TEST( matrix, diagMatrices ) +TEST(Matrix, diagMatrices ) { std::vector Hs; Hs.push_back(ones(3,3)); @@ -326,7 +328,7 @@ TEST( matrix, diagMatrices ) } /* ************************************************************************* */ -TEST( matrix, stream_read ) { +TEST(Matrix, stream_read ) { Matrix expected = (Matrix(3,4) << 1.1, 2.3, 4.2, 7.6, -0.3, -8e-2, 5.1, 9.0, @@ -346,7 +348,7 @@ TEST( matrix, stream_read ) { } /* ************************************************************************* */ -TEST( matrix, scale_columns ) +TEST(Matrix, scale_columns ) { Matrix A(3, 4); A(0, 0) = 1.; @@ -384,7 +386,7 @@ TEST( matrix, scale_columns ) } /* ************************************************************************* */ -TEST( matrix, scale_rows ) +TEST(Matrix, scale_rows ) { Matrix A(3, 4); A(0, 0) = 1.; @@ -422,7 +424,7 @@ TEST( matrix, scale_rows ) } /* ************************************************************************* */ -TEST( matrix, scale_rows_mask ) +TEST(Matrix, scale_rows_mask ) { Matrix A(3, 4); A(0, 0) = 1.; @@ -460,7 +462,7 @@ TEST( matrix, scale_rows_mask ) } /* ************************************************************************* */ -TEST( matrix, skewSymmetric ) +TEST(Matrix, skewSymmetric ) { double wx = 1, wy = 2, wz = 3; Matrix3 actual = skewSymmetric(wx,wy,wz); @@ -476,7 +478,7 @@ TEST( matrix, skewSymmetric ) /* ************************************************************************* */ -TEST( matrix, equal ) +TEST(Matrix, equal ) { Matrix A(4, 4); A(0, 0) = -1; @@ -506,7 +508,7 @@ TEST( matrix, equal ) } /* ************************************************************************* */ -TEST( matrix, equal_nan ) +TEST(Matrix, equal_nan ) { Matrix A(4, 4); A(0, 0) = -1; @@ -535,7 +537,7 @@ TEST( matrix, equal_nan ) } /* ************************************************************************* */ -TEST( matrix, addition ) +TEST(Matrix, addition ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -544,7 +546,7 @@ TEST( matrix, addition ) } /* ************************************************************************* */ -TEST( matrix, addition_in_place ) +TEST(Matrix, addition_in_place ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -554,7 +556,7 @@ TEST( matrix, addition_in_place ) } /* ************************************************************************* */ -TEST( matrix, subtraction ) +TEST(Matrix, subtraction ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -563,7 +565,7 @@ TEST( matrix, subtraction ) } /* ************************************************************************* */ -TEST( matrix, subtraction_in_place ) +TEST(Matrix, subtraction_in_place ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 4.0, 3.0, 2.0, 1.0).finished(); @@ -573,7 +575,7 @@ TEST( matrix, subtraction_in_place ) } /* ************************************************************************* */ -TEST( matrix, multiplication ) +TEST(Matrix, multiplication ) { Matrix A(2, 2); A(0, 0) = -1; @@ -593,7 +595,7 @@ TEST( matrix, multiplication ) } /* ************************************************************************* */ -TEST( matrix, scalar_matrix_multiplication ) +TEST(Matrix, scalar_matrix_multiplication ) { Vector result(2); @@ -613,7 +615,7 @@ TEST( matrix, scalar_matrix_multiplication ) } /* ************************************************************************* */ -TEST( matrix, matrix_vector_multiplication ) +TEST(Matrix, matrix_vector_multiplication ) { Vector result(2); @@ -627,7 +629,7 @@ TEST( matrix, matrix_vector_multiplication ) } /* ************************************************************************* */ -TEST( matrix, nrRowsAndnrCols ) +TEST(Matrix, nrRowsAndnrCols ) { Matrix A(3, 6); LONGS_EQUAL( A.rows() , 3 ); @@ -635,7 +637,7 @@ TEST( matrix, nrRowsAndnrCols ) } /* ************************************************************************* */ -TEST( matrix, scalar_divide ) +TEST(Matrix, scalar_divide ) { Matrix A(2, 2); A(0, 0) = 10; @@ -653,7 +655,7 @@ TEST( matrix, scalar_divide ) } /* ************************************************************************* */ -TEST( matrix, zero_below_diagonal ) { +TEST(Matrix, zero_below_diagonal ) { Matrix A1 = (Matrix(3, 4) << 1.0, 2.0, 3.0, 4.0, 1.0, 2.0, 3.0, 4.0, @@ -708,7 +710,7 @@ TEST( matrix, zero_below_diagonal ) { } /* ************************************************************************* */ -TEST( matrix, inverse ) +TEST(Matrix, inverse ) { Matrix A(3, 3); A(0, 0) = 1; @@ -754,7 +756,7 @@ TEST( matrix, inverse ) } /* ************************************************************************* */ -TEST( matrix, inverse2 ) +TEST(Matrix, inverse2 ) { Matrix A(3, 3); A(0, 0) = 0; @@ -784,7 +786,7 @@ TEST( matrix, inverse2 ) } /* ************************************************************************* */ -TEST( matrix, backsubtitution ) +TEST(Matrix, backsubtitution ) { // TEST ONE 2x2 matrix U1*x=b1 Vector expected1 = Vector2(3.6250, -0.75); @@ -809,7 +811,7 @@ TEST( matrix, backsubtitution ) } /* ************************************************************************* */ -TEST( matrix, householder ) +TEST(Matrix, householder ) { // check in-place householder, with v vectors below diagonal @@ -838,7 +840,7 @@ TEST( matrix, householder ) } /* ************************************************************************* */ -TEST( matrix, householder_colMajor ) +TEST(Matrix, householder_colMajor ) { // check in-place householder, with v vectors below diagonal @@ -867,7 +869,7 @@ TEST( matrix, householder_colMajor ) } /* ************************************************************************* */ -TEST( matrix, eigen_QR ) +TEST(Matrix, eigen_QR ) { // use standard Eigen function to yield a non-in-place QR factorization @@ -898,7 +900,7 @@ TEST( matrix, eigen_QR ) // unit test for qr factorization (and hence householder) // This behaves the same as QR in matlab: [Q,R] = qr(A), except for signs /* ************************************************************************* */ -TEST( matrix, qr ) +TEST(Matrix, qr ) { Matrix A = (Matrix(6, 4) << -5, 0, 5, 0, 00, -5, 0, 5, 10, 0, 0, 0, 00, 10, 0, 0, 00, @@ -921,7 +923,7 @@ TEST( matrix, qr ) } /* ************************************************************************* */ -TEST( matrix, sub ) +TEST(Matrix, sub ) { Matrix A = (Matrix(4, 6) << -5, 0, 5, 0, 0, 0, 00, -5, 0, 5, 0, 0, 10, 0, 0, 0, -10, 0, 00, 10, 0, 0, 0, -10).finished(); @@ -933,7 +935,7 @@ TEST( matrix, sub ) } /* ************************************************************************* */ -TEST( matrix, trans ) +TEST(Matrix, trans ) { Matrix A = (Matrix(2, 2) << 1.0, 3.0, 2.0, 4.0).finished(); Matrix B = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); @@ -941,7 +943,7 @@ TEST( matrix, trans ) } /* ************************************************************************* */ -TEST( matrix, col_major_access ) +TEST(Matrix, col_major_access ) { Matrix A = (Matrix(2, 2) << 1.0, 2.0, 3.0, 4.0).finished(); const double* a = &A(0, 0); @@ -949,7 +951,7 @@ TEST( matrix, col_major_access ) } /* ************************************************************************* */ -TEST( matrix, weighted_elimination ) +TEST(Matrix, weighted_elimination ) { // create a matrix to eliminate Matrix A = (Matrix(4, 6) << -1., 0., 1., 0., 0., 0., 0., -1., 0., 1., 0., 0., @@ -984,7 +986,7 @@ TEST( matrix, weighted_elimination ) } /* ************************************************************************* */ -TEST( matrix, inverse_square_root ) +TEST(Matrix, inverse_square_root ) { Matrix measurement_covariance = (Matrix(3, 3) << 0.25, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.01).finished(); @@ -1036,22 +1038,22 @@ Matrix expected = (Matrix(5, 5) << 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247).finished(); } -TEST( matrix, LLt ) +TEST(Matrix, LLt ) { EQUALITY(cholesky::expected, LLt(cholesky::M)); } -TEST( matrix, RtR ) +TEST(Matrix, RtR ) { EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M)); } -TEST( matrix, cholesky_inverse ) +TEST(Matrix, cholesky_inverse ) { EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M)); } /* ************************************************************************* */ -TEST( matrix, multiplyAdd ) +TEST(Matrix, multiplyAdd ) { Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), @@ -1062,7 +1064,7 @@ TEST( matrix, multiplyAdd ) } /* ************************************************************************* */ -TEST( matrix, transposeMultiplyAdd ) +TEST(Matrix, transposeMultiplyAdd ) { Matrix A = (Matrix(3, 4) << 4., 0., 0., 1., 0., 4., 0., 2., 0., 0., 1., 3.).finished(); Vector x = (Vector(4) << 1., 2., 3., 4.).finished(), e = Vector3(5., 6., 7.), @@ -1073,7 +1075,7 @@ TEST( matrix, transposeMultiplyAdd ) } /* ************************************************************************* */ -TEST( matrix, linear_dependent ) +TEST(Matrix, linear_dependent ) { Matrix A = (Matrix(2, 3) << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix B = (Matrix(2, 3) << -1.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); @@ -1081,7 +1083,7 @@ TEST( matrix, linear_dependent ) } /* ************************************************************************* */ -TEST( matrix, linear_dependent2 ) +TEST(Matrix, linear_dependent2 ) { Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.0, 10.0, 12.0).finished(); @@ -1089,7 +1091,7 @@ TEST( matrix, linear_dependent2 ) } /* ************************************************************************* */ -TEST( matrix, linear_dependent3 ) +TEST(Matrix, linear_dependent3 ) { Matrix A = (Matrix(2, 3) << 0.0, 2.0, 3.0, 4.0, 5.0, 6.0).finished(); Matrix B = (Matrix(2, 3) << 0.0, -2.0, -3.0, 8.1, 10.0, 12.0).finished(); @@ -1097,7 +1099,7 @@ TEST( matrix, linear_dependent3 ) } /* ************************************************************************* */ -TEST( matrix, svd1 ) +TEST(Matrix, svd1 ) { Vector v = Vector3(2., 1., 0.); Matrix U1 = eye(4, 3), S1 = diag(v), V1 = eye(3, 3), A = (U1 * S1) @@ -1116,7 +1118,7 @@ static Matrix sampleA = (Matrix(3, 2) << 0.,-2., 0., 0., 3., 0.).finished(); static Matrix sampleAt = trans(sampleA); /* ************************************************************************* */ -TEST( matrix, svd2 ) +TEST(Matrix, svd2 ) { Matrix U, V; Vector s; @@ -1139,7 +1141,7 @@ TEST( matrix, svd2 ) } /* ************************************************************************* */ -TEST( matrix, svd3 ) +TEST(Matrix, svd3 ) { Matrix U, V; Vector s; @@ -1167,7 +1169,7 @@ TEST( matrix, svd3 ) } /* ************************************************************************* */ -TEST( matrix, svd4 ) +TEST(Matrix, svd4 ) { Matrix U, V; Vector s; @@ -1209,7 +1211,7 @@ TEST( matrix, svd4 ) } /* ************************************************************************* */ -TEST( matrix, DLT ) +TEST(Matrix, DLT ) { Matrix A = (Matrix(8, 9) << 0.21, -0.42, -10.71, 0.18, -0.36, -9.18, -0.61, 1.22, 31.11, @@ -1231,6 +1233,18 @@ TEST( matrix, DLT ) EXPECT(assert_equal(expected, actual, 1e-4)); } +//****************************************************************************** +TEST(Matrix , IsVectorSpace) { + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + typedef Eigen::Matrix RowMajor; + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + typedef Eigen::Matrix RowVector; + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/base/tests/testNumericalDerivative.cpp b/gtsam/base/tests/testNumericalDerivative.cpp index d3a2b929f..1cbf71e1f 100644 --- a/gtsam/base/tests/testNumericalDerivative.cpp +++ b/gtsam/base/tests/testNumericalDerivative.cpp @@ -95,6 +95,12 @@ TEST(testNumericalDerivative, numericalHessian211) { EXPECT(assert_equal(expected22, actual22, 1e-5)); } +TEST(testNumericalDerivative, numericalHessian212) { + // TODO should implement test for all the variants of numerical Hessian, for mixed dimension types, + // like Point3 y = Project(Camera, Point3); + // I'm not sure how numericalHessian212 is different from 211 or 222 -Mike B. +} + /* ************************************************************************* */ double f4(double x, double y, double z) { return sin(x) * cos(y) * z * z; diff --git a/gtsam/base/tests/testOptionalJacobian.cpp b/gtsam/base/tests/testOptionalJacobian.cpp index 6584c82d1..331fb49eb 100644 --- a/gtsam/base/tests/testOptionalJacobian.cpp +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -46,6 +46,18 @@ TEST( OptionalJacobian, Constructors ) { boost::optional optional(dynamic); OptionalJacobian<2, 3> H6(optional); EXPECT(H6); + + OptionalJacobian<-1, -1> H7; + EXPECT(!H7); + + OptionalJacobian<-1, -1> H8(dynamic); + EXPECT(H8); + + OptionalJacobian<-1, -1> H9(boost::none); + EXPECT(!H9); + + OptionalJacobian<-1, -1> H10(optional); + EXPECT(H10); } //****************************************************************************** @@ -59,10 +71,9 @@ void testPtr(Matrix23* H = NULL) { *H = Matrix23::Zero(); } -TEST( OptionalJacobian, Ref2) { +TEST( OptionalJacobian, Fixed) { - Matrix expected; - expected = Matrix23::Zero(); + Matrix expected = Matrix23::Zero(); // Default argument does nothing test(); @@ -88,13 +99,44 @@ TEST( OptionalJacobian, Ref2) { Matrix dynamic1(3, 5); dynamic1.setOnes(); test(dynamic1); - EXPECT(assert_equal(expected,dynamic0)); + EXPECT(assert_equal(expected,dynamic1)); // Dynamic right size Matrix dynamic2(2, 5); dynamic2.setOnes(); test(dynamic2); - EXPECT(assert_equal(dynamic2,dynamic0)); + EXPECT(assert_equal(expected, dynamic2)); +} + +//****************************************************************************** +void test2(OptionalJacobian<-1,-1> H = boost::none) { + if (H) + *H = Matrix23::Zero(); // resizes +} + +TEST( OptionalJacobian, Dynamic) { + + Matrix expected = Matrix23::Zero(); + + // Default argument does nothing + test2(); + + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test2(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3, 5); + dynamic1.setOnes(); + test2(dynamic1); + EXPECT(assert_equal(expected,dynamic1)); + + // Dynamic right size + Matrix dynamic2(2, 5); + dynamic2.setOnes(); + test2(dynamic2); + EXPECT(assert_equal(expected, dynamic2)); } //****************************************************************************** diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 460ff6cd6..00e40039b 100644 --- a/gtsam/base/tests/testVector.cpp +++ b/gtsam/base/tests/testVector.cpp @@ -15,10 +15,12 @@ * @author Frank Dellaert **/ -#include +#include +#include +#include #include #include -#include +#include using namespace std; using namespace gtsam; @@ -40,7 +42,7 @@ namespace { } /* ************************************************************************* */ -TEST( TestVector, special_comma_initializer) +TEST(Vector, special_comma_initializer) { Vector expected(3); expected(0) = 1; @@ -68,7 +70,7 @@ TEST( TestVector, special_comma_initializer) } /* ************************************************************************* */ -TEST( TestVector, copy ) +TEST(Vector, copy ) { Vector a(2); a(0) = 10; a(1) = 20; double data[] = {10,20}; @@ -78,14 +80,14 @@ TEST( TestVector, copy ) } /* ************************************************************************* */ -TEST( TestVector, zero1 ) +TEST(Vector, zero1 ) { Vector v = Vector::Zero(2); EXPECT(zero(v)); } /* ************************************************************************* */ -TEST( TestVector, zero2 ) +TEST(Vector, zero2 ) { Vector a = zero(2); Vector b = Vector::Zero(2); @@ -94,7 +96,7 @@ TEST( TestVector, zero2 ) } /* ************************************************************************* */ -TEST( TestVector, scalar_multiply ) +TEST(Vector, scalar_multiply ) { Vector a(2); a(0) = 10; a(1) = 20; Vector b(2); b(0) = 1; b(1) = 2; @@ -102,7 +104,7 @@ TEST( TestVector, scalar_multiply ) } /* ************************************************************************* */ -TEST( TestVector, scalar_divide ) +TEST(Vector, scalar_divide ) { Vector a(2); a(0) = 10; a(1) = 20; Vector b(2); b(0) = 1; b(1) = 2; @@ -110,7 +112,7 @@ TEST( TestVector, scalar_divide ) } /* ************************************************************************* */ -TEST( TestVector, negate ) +TEST(Vector, negate ) { Vector a(2); a(0) = 10; a(1) = 20; Vector b(2); b(0) = -10; b(1) = -20; @@ -118,7 +120,7 @@ TEST( TestVector, negate ) } /* ************************************************************************* */ -TEST( TestVector, sub ) +TEST(Vector, sub ) { Vector a(6); a(0) = 10; a(1) = 20; a(2) = 3; @@ -134,7 +136,7 @@ TEST( TestVector, sub ) } /* ************************************************************************* */ -TEST( TestVector, subInsert ) +TEST(Vector, subInsert ) { Vector big = zero(6), small = ones(3); @@ -148,7 +150,7 @@ TEST( TestVector, subInsert ) } /* ************************************************************************* */ -TEST( TestVector, householder ) +TEST(Vector, householder ) { Vector x(4); x(0) = 3; x(1) = 1; x(2) = 5; x(3) = 1; @@ -163,7 +165,7 @@ TEST( TestVector, householder ) } /* ************************************************************************* */ -TEST( TestVector, concatVectors) +TEST(Vector, concatVectors) { Vector A(2); for(int i = 0; i < 2; i++) @@ -187,7 +189,7 @@ TEST( TestVector, concatVectors) } /* ************************************************************************* */ -TEST( TestVector, weightedPseudoinverse ) +TEST(Vector, weightedPseudoinverse ) { // column from a matrix Vector x(2); @@ -213,7 +215,7 @@ TEST( TestVector, weightedPseudoinverse ) } /* ************************************************************************* */ -TEST( TestVector, weightedPseudoinverse_constraint ) +TEST(Vector, weightedPseudoinverse_constraint ) { // column from a matrix Vector x(2); @@ -238,7 +240,7 @@ TEST( TestVector, weightedPseudoinverse_constraint ) } /* ************************************************************************* */ -TEST( TestVector, weightedPseudoinverse_nan ) +TEST(Vector, weightedPseudoinverse_nan ) { Vector a = (Vector(4) << 1., 0., 0., 0.).finished(); Vector sigmas = (Vector(4) << 0.1, 0.1, 0., 0.).finished(); @@ -252,7 +254,7 @@ TEST( TestVector, weightedPseudoinverse_nan ) } /* ************************************************************************* */ -TEST( TestVector, ediv ) +TEST(Vector, ediv ) { Vector a = Vector3(10., 20., 30.); Vector b = Vector3(2.0, 5.0, 6.0); @@ -263,7 +265,7 @@ TEST( TestVector, ediv ) } /* ************************************************************************* */ -TEST( TestVector, dot ) +TEST(Vector, dot ) { Vector a = Vector3(10., 20., 30.); Vector b = Vector3(2.0, 5.0, 6.0); @@ -271,7 +273,7 @@ TEST( TestVector, dot ) } /* ************************************************************************* */ -TEST( TestVector, axpy ) +TEST(Vector, axpy ) { Vector x = Vector3(10., 20., 30.); Vector y0 = Vector3(2.0, 5.0, 6.0); @@ -284,7 +286,7 @@ TEST( TestVector, axpy ) } /* ************************************************************************* */ -TEST( TestVector, equals ) +TEST(Vector, equals ) { Vector v1 = (Vector(1) << 0.0/std::numeric_limits::quiet_NaN()).finished(); //testing nan Vector v2 = (Vector(1) << 1.0).finished(); @@ -293,7 +295,7 @@ TEST( TestVector, equals ) } /* ************************************************************************* */ -TEST( TestVector, greater_than ) +TEST(Vector, greater_than ) { Vector v1 = Vector3(1.0, 2.0, 3.0), v2 = zero(3); @@ -302,14 +304,14 @@ TEST( TestVector, greater_than ) } /* ************************************************************************* */ -TEST( TestVector, reciprocal ) +TEST(Vector, reciprocal ) { Vector v = Vector3(1.0, 2.0, 4.0); EXPECT(assert_equal(Vector3(1.0, 0.5, 0.25),reciprocal(v))); } /* ************************************************************************* */ -TEST( TestVector, linear_dependent ) +TEST(Vector, linear_dependent ) { Vector v1 = Vector3(1.0, 2.0, 3.0); Vector v2 = Vector3(-2.0, -4.0, -6.0); @@ -317,7 +319,7 @@ TEST( TestVector, linear_dependent ) } /* ************************************************************************* */ -TEST( TestVector, linear_dependent2 ) +TEST(Vector, linear_dependent2 ) { Vector v1 = Vector3(0.0, 2.0, 0.0); Vector v2 = Vector3(0.0, -4.0, 0.0); @@ -325,13 +327,21 @@ TEST( TestVector, linear_dependent2 ) } /* ************************************************************************* */ -TEST( TestVector, linear_dependent3 ) +TEST(Vector, linear_dependent3 ) { Vector v1 = Vector3(0.0, 2.0, 0.0); Vector v2 = Vector3(0.1, -4.1, 0.0); EXPECT(!linear_dependent(v1, v2)); } +//****************************************************************************** +TEST(Vector, IsVectorSpace) { + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + typedef Eigen::Matrix RowVector; + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index dc7466199..f7253ec24 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -167,4 +167,7 @@ namespace gtsam { }; // DecisionTreeFactor +// traits +template<> struct traits : public Testable {}; + }// namespace gtsam diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index 8cb2db182..de5ec8042 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -95,5 +95,8 @@ namespace gtsam { } }; -} // namespace +// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam diff --git a/gtsam/discrete/DiscreteConditional.h b/gtsam/discrete/DiscreteConditional.h index 1ba97444f..88c3e5620 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -130,6 +130,9 @@ public: }; // DiscreteConditional +// traits +template<> struct traits : public Testable {}; + /* ************************************************************************* */ template DiscreteConditional::shared_ptr DiscreteConditional::Combine( diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 8351b310b..e24dfdf2a 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { @@ -101,4 +102,8 @@ public: }; // DiscreteFactor +// traits +template<> struct traits : public Testable {}; +template<> struct traits : public Testable {}; + }// namespace gtsam diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 27eb722d9..c5b11adf1 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -144,7 +144,10 @@ public: // // /** Apply a reduction, which is a remapping of variable indices. */ // GTSAM_EXPORT void reduceWithInverse(const internal::Reduction& inverseReduction); -}; -// DiscreteFactorGraph -} // namespace gtsam +}; // \ DiscreteFactorGraph + +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam diff --git a/gtsam/discrete/Potentials.h b/gtsam/discrete/Potentials.h index 51475e07d..978781d63 100644 --- a/gtsam/discrete/Potentials.h +++ b/gtsam/discrete/Potentials.h @@ -88,4 +88,9 @@ namespace gtsam { }; // Potentials +// traits +template<> struct traits : public Testable {}; +template<> struct traits : public Testable {}; + + } // namespace gtsam diff --git a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp index f8bcb45c2..970a18b42 100644 --- a/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp +++ b/gtsam/discrete/tests/testAlgebraicDecisionTree.cpp @@ -40,6 +40,11 @@ using namespace gtsam; /* ******************************************************************************** */ typedef AlgebraicDecisionTree ADT; +// traits +namespace gtsam { +template<> struct traits : public Testable {}; +} + #define DISABLE_DOT template diff --git a/gtsam/discrete/tests/testDecisionTree.cpp b/gtsam/discrete/tests/testDecisionTree.cpp index 1dfd56eec..05223c67a 100644 --- a/gtsam/discrete/tests/testDecisionTree.cpp +++ b/gtsam/discrete/tests/testDecisionTree.cpp @@ -44,12 +44,22 @@ void dot(const T&f, const string& filename) { struct Crazy { int a; double b; }; typedef DecisionTree CrazyDecisionTree; // check that DecisionTree is actually generic (as it pretends to be) +// traits +namespace gtsam { +template<> struct traits : public Testable {}; +} + /* ******************************************************************************** */ // Test string labels and int range /* ******************************************************************************** */ typedef DecisionTree DT; +// traits +namespace gtsam { +template<> struct traits
: public Testable
{}; +} + struct Ring { static inline int zero() { return 0; diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index fc1d63e10..ce9f94c48 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -37,6 +37,8 @@ private: public: + enum { dimension = 3 }; + /// @name Standard Constructors /// @{ @@ -169,24 +171,7 @@ private: }; -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static Cal3Bundler value() { - return Cal3Bundler(0, 0, 0); - } -}; - -} +struct traits : public internal::Manifold {}; } // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index fb338431a..0c77eebbc 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -43,6 +43,8 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { public: + enum { dimension = 9 }; + /// @name Standard Constructors /// @{ @@ -107,18 +109,8 @@ private: }; -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -} +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index e2aa3fc8b..d0e0f3891 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -50,6 +50,7 @@ private: double xi_; // mirror parameter public: + enum { dimension = 10 }; Vector10 vector() const ; @@ -138,23 +139,8 @@ private: }; -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static Cal3Unified value() { return Cal3Unified();} -}; - -} +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index e6b56eea0..0c5386822 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -36,7 +36,7 @@ private: double fx_, fy_, s_, u0_, v0_; public: - + enum { dimension = 5 }; typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object /// @name Standard Constructors @@ -223,22 +223,7 @@ private: }; -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static Cal3_S2 value() { return Cal3_S2();} -}; - -} +struct traits : public internal::Manifold {}; } // \ namespace gtsam diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index 68732ea8e..3585fb156 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -34,6 +34,7 @@ namespace gtsam { public: + enum { dimension = 6 }; typedef boost::shared_ptr shared_ptr; ///< shared pointer to stereo calibration object /// @name Standard Constructors @@ -153,21 +154,8 @@ namespace gtsam { }; // Define GTSAM traits - namespace traits { - template<> - struct GTSAM_EXPORT is_manifold : public boost::true_type{ + struct traits : public internal::Manifold { }; - template<> - struct GTSAM_EXPORT dimension : public boost::integral_constant{ - }; - - template<> - struct GTSAM_EXPORT zero { - static Cal3_S2Stereo value() { return Cal3_S2Stereo();} - }; - - } - } // \ namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index f5a8b4469..cda01b600 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -44,6 +44,8 @@ private: public: + enum { dimension = 6 }; + /// @name Standard Constructors /// @{ @@ -199,23 +201,8 @@ private: /// @} }; -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_group : public boost::true_type { -}; - -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type { -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant< - int, 6> { -}; - -} +struct traits : public internal::Manifold {}; } diff --git a/gtsam/base/Lie-inl.h b/gtsam/geometry/Cyclic.cpp similarity index 53% rename from gtsam/base/Lie-inl.h rename to gtsam/geometry/Cyclic.cpp index f1bb947a2..7242459a6 100644 --- a/gtsam/base/Lie-inl.h +++ b/gtsam/geometry/Cyclic.cpp @@ -10,19 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file Lie-inl.h - * @date Jan 9, 2010 - * @author Richard Roberts - * @brief Instantiate macro for Lie type - */ + * @file Cyclic.cpp + * @brief Cyclic group implementation + * @author Frank Dellaert + **/ -#pragma once - -#include - -#define INSTANTIATE_LIE(T) \ - template T between_default(const T&, const T&); \ - template Vector logmap_default(const T&, const T&); \ - template T expmap_default(const T&, const Vector&); +#include +namespace gtsam { +} // \namespace gtsam diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h new file mode 100644 index 000000000..2c883707f --- /dev/null +++ b/gtsam/geometry/Cyclic.h @@ -0,0 +1,83 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cyclic.h + * @brief Cyclic group, i.e., the integers modulo N + * @author Frank Dellaert + **/ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Cyclic group of order N +template +class Cyclic { + size_t i_; ///< we just use an unsigned int +public: + /// Constructor + Cyclic(size_t i) : + i_(i) { + assert(i < N); + } + /// Idenity element + static Cyclic Identity() { + return Cyclic(0); + } + /// Cast to size_t + operator size_t() const { + return i_; + } + /// Addition modulo N + Cyclic operator+(const Cyclic& h) const { + return (i_ + h.i_) % N; + } + /// Subtraction modulo N + Cyclic operator-(const Cyclic& h) const { + return (N + i_ - h.i_) % N; + } + /// Inverse + Cyclic operator-() const { + return (N - i_) % N; + } + /// print with optional string + void print(const std::string& s = "") const { + std::cout << s << i_ << std::endl; + } + /// equals with an tolerance, prints out message if unequal + bool equals(const Cyclic& other, double tol = 1e-9) const { + return other.i_ == i_; + } +}; + +/// Define cyclic group traits to be a model of the Group concept +template +struct traits > { + typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic) + static Cyclic Identity() { + return Cyclic::Identity(); + } + static bool Equals(const Cyclic& a, const Cyclic& b, + double tol = 1e-9) { + return a.equals(b, tol); + } + static void Print(const Cyclic& c, const std::string &s = "") { + c.print(s); + } +}; + +} // \namespace gtsam + diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 546e432b9..c9e702078 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -30,6 +30,8 @@ private: public: + enum { dimension = 5 }; + /// Static function to convert Point2 to homogeneous coordinates static Vector3 Homogeneous(const Point2& p) { return Vector3(p.x(), p.y(), 1); @@ -196,23 +198,8 @@ private: }; -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static EssentialMatrix value() { return EssentialMatrix();} -}; - -} +struct traits : public internal::Manifold {}; } // gtsam diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 9b881dc78..b409c956d 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -31,16 +31,20 @@ namespace gtsam { */ template class PinholeCamera { + private: Pose3 pose_; Calibration K_; - // Get dimensions of calibration type and This at compile time - static const int DimK = traits::dimension::value, // - DimC = 6 + DimK; + GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) + + // Get dimensions of calibration type at compile time + static const int DimK = FixedDimension::value; public: + enum { dimension = 6 + DimK }; + /// @name Standard Constructors /// @{ @@ -168,15 +172,15 @@ public: /// Manifold dimension inline size_t dim() const { - return DimC; + return dimension; } /// Manifold dimension inline static size_t Dim() { - return DimC; + return dimension; } - typedef Eigen::Matrix VectorK6; + typedef Eigen::Matrix VectorK6; /// move a cameras according to d PinholeCamera retract(const Vector& d) const { @@ -189,12 +193,18 @@ public: /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { - VectorK6 d; // TODO: why does d.head<6>() not compile?? + VectorK6 d; + // TODO: why does d.head<6>() not compile?? d.head(6) = pose().localCoordinates(T2.pose()); d.tail(DimK) = calibration().localCoordinates(T2.calibration()); return d; } + /// for Canonical + static PinholeCamera identity() { + return PinholeCamera(); // assumes that the default constructor is valid + } + /// @} /// @name Transformations and measurement functions /// @{ @@ -309,7 +319,7 @@ public: */ Point2 project2( const Point3& pw, // - OptionalJacobian<2, DimC> Dcamera = boost::none, + OptionalJacobian<2, dimension> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); @@ -359,7 +369,7 @@ public: */ double range( const Point3& point, // - OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { Matrix16 Dpose_; double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); @@ -377,7 +387,7 @@ public: */ double range( const Pose3& pose, // - OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const { Matrix16 Dpose_; double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); @@ -396,7 +406,7 @@ public: template double range( const PinholeCamera& camera, // - OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, dimension> Dcamera = boost::none, // OptionalJacobian<1, 6 + traits::dimension::value> Dother = boost::optional Dother = boost::none) const { @@ -408,7 +418,7 @@ public: *Dcamera << Dcamera_, Eigen::Matrix::Zero(); } if (Dother) { - Dother->resize(1, 6+traits::dimension::value); + Dother->resize(1, 6+CalibrationB::dimension); Dother->setZero(); Dother->block(0, 0, 1, 6) = Dother_; } @@ -424,7 +434,7 @@ public: */ double range( const CalibratedCamera& camera, // - OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { return range(camera.pose(), Dcamera, Dother); } @@ -485,26 +495,8 @@ private: }; -// Define GTSAM traits -namespace traits { template -struct GTSAM_EXPORT is_manifold > : public boost::true_type { -}; - -template -struct GTSAM_EXPORT dimension > : public boost::integral_constant< - int, dimension::value + dimension::value> { -}; - -template -struct GTSAM_EXPORT zero > { - static PinholeCamera value() { - return PinholeCamera(zero::value(), - zero::value()); - } -}; - -} +struct traits< PinholeCamera > : public internal::Manifold > {}; } // \ gtsam diff --git a/gtsam/geometry/Point2.cpp b/gtsam/geometry/Point2.cpp index 17e3ef370..4b2111efc 100644 --- a/gtsam/geometry/Point2.cpp +++ b/gtsam/geometry/Point2.cpp @@ -16,7 +16,6 @@ */ #include -#include #include #include @@ -24,9 +23,6 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Point2); - /* ************************************************************************* */ void Point2::print(const string& s) const { cout << s << *this << endl; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index da630a803..01a6d6052 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -17,12 +17,9 @@ #pragma once +#include #include -#include -#include -#include - namespace gtsam { /** @@ -33,13 +30,12 @@ namespace gtsam { * \nosubgrouping */ class GTSAM_EXPORT Point2 { - private: double x_, y_; public: - + enum { dimension = 2 }; /// @name Standard Constructors /// @{ @@ -54,9 +50,7 @@ public: /// @{ /// construct from 2D vector - Point2(const Vector& v) { - if(v.size() != 2) - throw std::invalid_argument("Point2 constructor from Vector requires that the Vector have dimension 2"); + Point2(const Vector2& v) { x_ = v(0); y_ = v(1); } @@ -113,72 +107,17 @@ public: /// @{ /// identity - inline static Point2 identity() { - return Point2(); - } + inline static Point2 identity() {return Point2();} - /// "Inverse" - negates each coordinate such that compose(p,inverse(p)) == identity() - inline Point2 inverse() const { return Point2(-x_, -y_); } - - /// syntactic sugar for inverse, i.e., -p == inverse(p) + /// inverse inline Point2 operator- () const {return Point2(-x_,-y_);} - /// "Compose", just adds the coordinates of two points. With optional derivatives - inline Point2 compose(const Point2& q, - OptionalJacobian<2,2> H1=boost::none, - OptionalJacobian<2,2> H2=boost::none) const { - if(H1) *H1 = I_2x2; - if(H2) *H2 = I_2x2; - return *this + q; - } - - /// syntactic sugar for adding two points, i.e., p+q == compose(p,q) + /// add inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - /// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q) - inline Point2 between(const Point2& q, - OptionalJacobian<2,2> H1=boost::none, - OptionalJacobian<2,2> H2=boost::none) const { - if(H1) *H1 = -I_2x2; - if(H2) *H2 = I_2x2; - return q - (*this); - } - - /// syntactic sugar for subtracting points, i.e., q-p == between(p,q) + /// subtract inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return 2; } - - /// Dimensionality of tangent space = 2 DOF - inline size_t dim() const { return 2; } - - /// Updates a with tangent space delta - inline Point2 retract(const Vector& v) const { return *this + Point2(v); } - - /// Local coordinates of manifold neighborhood around current value - inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } - - /// @} - /// @name Lie Group - /// @{ - - /// Exponential map around identity - just create a Point2 from a vector - static inline Point2 Expmap(const Vector& v) { return Point2(v); } - - /// Log map around identity - just return the Point2 as a vector - static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); } - - /// Left-trivialized derivative of the exponential map - static Matrix ExpmapDerivative(const Vector2& v) {return I_2x2;} - - /// Left-trivialized derivative inverse of the exponential map - static Matrix LogmapDerivative(const Vector2& v) { return I_2x2;} - /// @} /// @name Vector Space /// @{ @@ -221,10 +160,18 @@ public: Vector2 vector() const { return Vector2(x_, y_); } /// @} - /// @name Deprecated (non-const, non-functional style. Do not use). + + /// @name Deprecated /// @{ inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} inline void operator *= (double s) {x_*=s;y_*=s;} + Point2 inverse() { return -(*this);} + Point2 compose(const Point2& q) { return (*this)+q;} + Point2 between(const Point2& q) { return q-(*this);} + Vector2 localCoordinates(const Point2& q) { return between(q).vector();} + Point2 retract(const Vector2& v) {return compose(Point2(v));} + static Vector2 Logmap(const Point2& p) {return p.vector();} + static Point2 Expmap(const Vector2& v) { return Point2(v);} /// @} /// Streaming @@ -251,22 +198,8 @@ private: /// multiply with scalar inline Point2 operator*(double s, const Point2& p) {return p*s;} -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_group : public boost::true_type{ -}; +struct traits : public internal::VectorSpace {}; -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -} - -} +} // \ namespace gtsam diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 330fafb97..a87aeb650 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -15,15 +15,11 @@ */ #include -#include using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Point3); - /* ************************************************************************* */ bool Point3::equals(const Point3 & q, double tol) const { return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 0afe2acf8..5b133dc60 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -21,12 +21,8 @@ #pragma once -#include -#include -#include - +#include #include - #include namespace gtsam { @@ -43,6 +39,7 @@ namespace gtsam { double x_, y_, z_; public: + enum { dimension = 3 }; /// @name Standard Constructors /// @{ @@ -58,9 +55,7 @@ namespace gtsam { /// @{ /// Construct from 3-element vector - Point3(const Vector& v) { - if(v.size() != 3) - throw std::invalid_argument("Point3 constructor from Vector requires that the Vector have dimension 3"); + Point3(const Vector3& v) { x_ = v(0); y_ = v(1); z_ = v(2); @@ -81,76 +76,17 @@ namespace gtsam { /// @{ /// identity for group operation - inline static Point3 identity() { - return Point3(); - } + inline static Point3 identity() { return Point3();} - /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() - inline Point3 inverse() const { return Point3(-x_, -y_, -z_); } - - /// syntactic sugar for inverse, i.e., -p == inverse(p) + /// inverse Point3 operator - () const { return Point3(-x_,-y_,-z_);} - /// "Compose" - just adds coordinates of two points - inline Point3 compose(const Point3& p2, - OptionalJacobian<3,3> H1=boost::none, - OptionalJacobian<3,3> H2=boost::none) const { - if (H1) *H1 << I_3x3; - if (H2) *H2 << I_3x3; - return *this + p2; - } - - ///syntactic sugar for adding two points, i.e., p+q == compose(p,q) + /// add Point3 operator + (const Point3& q) const; - /** Between using the default implementation */ - inline Point3 between(const Point3& p2, - OptionalJacobian<3,3> H1=boost::none, - OptionalJacobian<3,3> H2=boost::none) const { - if(H1) *H1 = -I_3x3; - if(H2) *H2 = I_3x3; - return p2 - *this; - } - - /// syntactic sugar for subtracting points, i.e., q-p == between(p,q) + /// subtract Point3 operator - (const Point3& q) const; - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return 3; } - - /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return 3; } - - /// Updates a with tangent space delta - inline Point3 retract(const Vector& v) const { return Point3(*this + v); } - - /// Returns inverse retraction - inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } - - /// @} - /// @name Lie Group - /// @{ - - /** Exponential map at identity - just create a Point3 from x,y,z */ - static inline Point3 Expmap(const Vector& v) { return Point3(v); } - - /** Log map at identity - return the x,y,z of this point */ - static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } - - /// Left-trivialized derivative of the exponential map - static Matrix3 ExpmapDerivative(const Vector& v) { - return I_3x3; - } - - /// Left-trivialized derivative inverse of the exponential map - static Matrix3 LogmapDerivative(const Vector& v) { - return I_3x3; - } - /// @} /// @name Vector Space /// @{ @@ -226,6 +162,17 @@ namespace gtsam { /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); + /// @name Deprecated + /// @{ + Point3 inverse() { return -(*this);} + Point3 compose(const Point3& q) { return (*this)+q;} + Point3 between(const Point3& q) { return q-(*this);} + Vector3 localCoordinates(const Point3& q) { return between(q).vector();} + Point3 retract(const Vector3& v) {return compose(Point3(v));} + static Vector3 Logmap(const Point3& p) {return p.vector();} + static Point3 Expmap(const Vector3& v) { return Point3(v);} + /// @} + private: /// @name Advanced Interface @@ -248,20 +195,6 @@ namespace gtsam { /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} - // Define GTSAM traits - namespace traits { - template<> - struct GTSAM_EXPORT is_group : public boost::true_type{ - }; - - template<> - struct GTSAM_EXPORT is_manifold : public boost::true_type{ - }; - - template<> - struct GTSAM_EXPORT dimension : public boost::integral_constant{ - }; - - } + struct traits : public internal::VectorSpace {}; } diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index a2dfc9f18..0b0172857 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -16,9 +16,11 @@ #include #include -#include #include +#include + #include + #include #include #include @@ -27,9 +29,6 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Pose2); - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose2); @@ -60,7 +59,8 @@ bool Pose2::equals(const Pose2& q, double tol) const { } /* ************************************************************************* */ -Pose2 Pose2::Expmap(const Vector& xi) { +Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) { + if (H) *H = Pose2::ExpmapDerivative(xi); assert(xi.size() == 3); Point2 v(xi(0),xi(1)); double w = xi(2); @@ -75,7 +75,8 @@ Pose2 Pose2::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector3 Pose2::Logmap(const Pose2& p) { +Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) { + if (H) *H = Pose2::LogmapDerivative(p); const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); @@ -91,21 +92,26 @@ Vector3 Pose2::Logmap(const Pose2& p) { } /* ************************************************************************* */ -Pose2 Pose2::retract(const Vector& v) const { +Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) { #ifdef SLOW_BUT_CORRECT_EXPMAP - return compose(Expmap(v)); + return Expmap(v, H); #else - assert(v.size() == 3); - return compose(Pose2(v[0], v[1], v[2])); + if (H) { + *H = I_3x3; + H->topLeftCorner<2,2>() = Rot2(-v[2]).matrix(); + } + return Pose2(v[0], v[1], v[2]); #endif } - /* ************************************************************************* */ -Vector3 Pose2::localCoordinates(const Pose2& p2) const { +Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) { #ifdef SLOW_BUT_CORRECT_EXPMAP - return Logmap(between(p2)); + return Logmap(r, H); #else - Pose2 r = between(p2); + if (H) { + *H = I_3x3; + H->topLeftCorner<2,2>() = r.rotation().matrix(); + } return Vector3(r.x(), r.y(), r.theta()); #endif } @@ -166,7 +172,8 @@ Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { } /* ************************************************************************* */ -Matrix3 Pose2::LogmapDerivative(const Vector3& v) { +Matrix3 Pose2::LogmapDerivative(const Pose2& p) { + Vector3 v = Logmap(p); double alpha = v[2]; Matrix3 J; if (fabs(alpha) > 1e-5) { @@ -187,8 +194,7 @@ Matrix3 Pose2::LogmapDerivative(const Vector3& v) { } /* ************************************************************************* */ -Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const { - if (H1) *H1 = -AdjointMap(); +Pose2 Pose2::inverse() const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } @@ -206,16 +212,6 @@ Point2 Pose2::transform_to(const Point2& point, return q; } -/* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1, - OptionalJacobian<3,3> H2) const { - // TODO: inline and reuse? - if(H1) *H1 = p2.inverse().AdjointMap(); - if(H2) *H2 = I_3x3; - return (*this)*p2; -} - /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_from(const Point2& p, @@ -231,40 +227,6 @@ Point2 Pose2::transform_from(const Point2& p, return q + t_; } -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1, - OptionalJacobian<3,3> H2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above - if (H1) { - double dt1 = -s2 * x + c2 * y; - double dt2 = -c2 * x - s2 * y; - *H1 << - -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0,-1.0; - } - if (H2) *H2 = I_3x3; - - return Pose2(R,t); -} - /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 32046cfcb..be860107e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -20,11 +20,9 @@ #pragma once -#include -#include -#include #include #include +#include namespace gtsam { @@ -33,7 +31,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose2 { +class GTSAM_EXPORT Pose2: public LieGroup { public: @@ -42,6 +40,7 @@ public: typedef Point2 Translation; private: + Rot2 r_; Point2 t_; @@ -106,50 +105,23 @@ public: /// identity for group operation inline static Pose2 identity() { return Pose2(); } - /// inverse transformation with derivatives - Pose2 inverse(OptionalJacobian<3, 3> H1=boost::none) const; - - /// compose this transformation onto another (first *this and then p2) - Pose2 compose(const Pose2& p2, - OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; + /// inverse + Pose2 inverse() const; /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { return Pose2(r_*p2.r(), t_ + r_*p2.t()); } - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none, - OptionalJacobian<3,3> H = boost::none) const; - - /// @} - /// @name Manifold - /// @{ - - /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes - inline static size_t Dim() { return 3; } - - /// Dimensionality of tangent space = 3 DOF - inline size_t dim() const { return 3; } - - /// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose - Pose2 retract(const Vector& v) const; - - /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose - Vector3 localCoordinates(const Pose2& p2) const; - /// @} /// @name Lie Group /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - static Pose2 Expmap(const Vector& xi); + static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - static Vector3 Logmap(const Pose2& p); + static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); /** * Calculate Adjoint map @@ -185,8 +157,15 @@ public: static Matrix3 ExpmapDerivative(const Vector3& v); /// Derivative of Logmap - static Matrix3 LogmapDerivative(const Vector3& v); + static Matrix3 LogmapDerivative(const Pose2& v); + // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP + struct ChartAtOrigin { + static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); + static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); + }; + + using LieGroup::inverse; // version with derivative /// @} /// @name Group Action on Point2 @@ -311,18 +290,8 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point2Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -} +struct traits : public internal::LieGroupTraits {}; } // namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index f5b840d08..be8e1bfed 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -17,7 +17,8 @@ #include #include #include -#include +#include + #include #include #include @@ -26,9 +27,6 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Pose3); - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); @@ -38,6 +36,12 @@ Pose3::Pose3(const Pose2& pose2) : Point3(pose2.x(), pose2.y(), 0)) { } +/* ************************************************************************* */ +Pose3 Pose3::inverse() const { + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt * (-t_)); +} + /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) @@ -107,7 +111,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector& xi) { +Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { + if (H) *H = ExpmapDerivative(xi); // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -127,7 +132,8 @@ Pose3 Pose3::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& p) { +Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { + if (H) *H = LogmapDerivative(p); Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = w.norm(); if (t < 1e-10) { @@ -148,57 +154,35 @@ Vector6 Pose3::Logmap(const Pose3& p) { } /* ************************************************************************* */ -Pose3 Pose3::retractFirstOrder(const Vector& xi) const { - Vector3 omega(sub(xi, 0, 3)); - Point3 v(sub(xi, 3, 6)); - Rot3 R = R_.retract(omega); // R is done exactly - Point3 t = t_ + R_ * v; // First order t approximation - return Pose3(R, t); +Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, H); +#else + Matrix3 DR; + Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0); + if (H) { + *H = I_6x6; + H->topLeftCorner<3,3>() = DR; + } + return Pose3(R, Point3(xi.tail<3>())); +#endif } /* ************************************************************************* */ -// Different versions of retract -Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group exponential map, traces out geodesic - return compose(Expmap(xi)); - } else if (mode == Pose3::FIRST_ORDER) { - // First order - return retractFirstOrder(xi); - } else { - // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently - // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation - assert(false); - exit(1); - } -} - -/* ************************************************************************* */ -// different versions of localCoordinates -Vector6 Pose3::localCoordinates(const Pose3& T, - Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group logarithm map, exact inverse of exponential map - return Logmap(between(T)); - } else if (mode == Pose3::FIRST_ORDER) { - // R is always done exactly in all three retract versions below - Vector3 omega = R_.localCoordinates(T.rotation()); - - // Incorrect version - // Independently computes the logmap of the translation and rotation - // Vector v = t_.localCoordinates(T.translation()); - - // Correct first order t inverse - Point3 d = R_.unrotate(T.translation() - t_); - - // TODO: correct second order t inverse - Vector6 local; - local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z(); - return local; - } else { - assert(false); - exit(1); +Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(T, H); +#else + Matrix3 DR; + Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); + if (H) { + *H = I_6x6; + H->topLeftCorner<3,3>() = DR; } + Vector6 xi; + xi << omega, T.translation().vector(); + return xi; +#endif } /* ************************************************************************* */ @@ -231,12 +215,20 @@ static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { } #else // The closed-form formula in Barfoot14tro eq. (102) - double phi = w.norm(), sinPhi = sin(phi), cosPhi = cos(phi), phi2 = phi * phi, - phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; - // Invert the sign of odd-order terms to have the right Jacobian - Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) - + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) - - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6)/phi5)*(W*V*W*W + W*W*V*W); + double phi = w.norm(); + if (fabs(phi)>1e-5) { + double sinPhi = sin(phi), cosPhi = cos(phi); + double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); + } + else { + Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) + + 1./24.*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + } #endif return Q; @@ -252,7 +244,8 @@ Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { } /* ************************************************************************* */ -Matrix6 Pose3::LogmapDerivative(const Vector6& xi) { +Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { + Vector6 xi = Logmap(pose); Vector3 w(sub(xi, 0, 3)); Matrix3 Jw = Rot3::LogmapDerivative(w); Matrix3 Q = computeQforExpmapDerivative(xi); @@ -311,31 +304,6 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, return q; } -/* ************************************************************************* */ -Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, - OptionalJacobian<6,6> H2) const { - if (H1) *H1 = p2.inverse().AdjointMap(); - if (H2) *H2 = I_6x6; - return (*this) * p2; -} - -/* ************************************************************************* */ -Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const { - if (H1) *H1 = -AdjointMap(); - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt * (-t_)); -} - -/* ************************************************************************* */ -// between = compose(p2,inverse(p1)); -Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, - OptionalJacobian<6,6> H2) const { - Pose3 result = inverse() * p2; - if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = I_6x6; - return result; -} - /* ************************************************************************* */ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) const { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 39d74bd9b..d930c815e 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -19,15 +19,9 @@ #include -#ifndef GTSAM_POSE3_EXPMAP -#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER -#else -#define POSE3_DEFAULT_COORDINATES_MODE Pose3::EXPMAP -#endif - -#include #include #include +#include namespace gtsam { @@ -39,7 +33,7 @@ class Pose2; * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Pose3{ +class GTSAM_EXPORT Pose3: public LieGroup { public: /** Pose Concept requirements */ @@ -48,7 +42,7 @@ public: private: - Rot3 R_; ///< Rotation gRp, between global and pose frame + Rot3 R_; ///< Rotation gRp, between global and pose frame Point3 t_; ///< Translation gTp, from global origin to pose frame origin public: @@ -75,7 +69,6 @@ public: R_(R), t_(t) { } - /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); @@ -105,239 +98,218 @@ public: } /// inverse transformation with derivatives - Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const; - - ///compose this transformation onto another (first *this and then p2) - Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, - OptionalJacobian<6, 6> H2 = boost::none) const; + Pose3 inverse() const; /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { return Pose3(R_ * T.R_, t_ + R_ * T.t_); } - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - * as well as optionally the derivatives - */ - Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, - OptionalJacobian<6, 6> H2 = boost::none) const; - /// @} - /// @name Manifold + /// @name Lie Group /// @{ - /** Enum to indicate which method should be used in Pose3::retract() and - * Pose3::localCoordinates() + /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ + static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none); + + /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation + static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); + + /** + * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame + * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ - enum CoordinatesMode { - EXPMAP, ///< The correct exponential map, computationally expensive. - FIRST_ORDER ///< A fast first-order approximation to the exponential map. + Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect + + /** + * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + */ + Vector Adjoint(const Vector& xi_b) const { + return AdjointMap() * xi_b; + } /// FIXME Not tested - marked as incorrect + + /** + * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 + * [ad(w,v)] = [w^, zero3; v^, w^] + * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, + * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. + * + * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its + * vector representation. + * We have the following relationship: + * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ + * + * We use this to compute the discrete version of the inverse right-trivialized tangent map, + * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. + * + */ + static Matrix6 adjointMap(const Vector6 &xi); + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, + OptionalJacobian<6, 6> = boost::none); + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, + OptionalJacobian<6, 6> H = boost::none); + + /// Derivative of Expmap + static Matrix6 ExpmapDerivative(const Vector6& xi); + + /// Derivative of Logmap + static Matrix6 LogmapDerivative(const Pose3& xi); + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct ChartAtOrigin { + static Pose3 Retract(const Vector6& v, ChartJacobian H = boost::none); + static Vector6 Local(const Pose3& r, ChartJacobian H = boost::none); }; - /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes - static size_t Dim() { - return 6; - } - - /// Dimensionality of the tangent space = 6 DOF - size_t dim() const { - return 6; - } - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map - Pose3 retractFirstOrder(const Vector& d) const; - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose - Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = - POSE3_DEFAULT_COORDINATES_MODE) const; - - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const; - - /// @} - /// @name Lie Group - /// @{ - - /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ - static Pose3 Expmap(const Vector& xi); - - /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation - static Vector6 Logmap(const Pose3& p); - - /** - * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame - * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) - */ - Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect - - /** - * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame - * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ - */ - Vector Adjoint(const Vector& xi_b) const {return AdjointMap()*xi_b; } /// FIXME Not tested - marked as incorrect - - /** - * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 - * [ad(w,v)] = [w^, zero3; v^, w^] - * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, - * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. - * - * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its - * vector representation. - * We have the following relationship: - * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ - * - * We use this to compute the discrete version of the inverse right-trivialized tangent map, - * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. - * - */ - static Matrix6 adjointMap(const Vector6 &xi); - - /** - * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives - */ - static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none); - - /** - * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. - */ - static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); - -public: - - /// Derivative of Expmap - static Matrix6 ExpmapDerivative(const Vector6& xi); - - /// Derivative of Logmap - static Matrix6 LogmapDerivative(const Vector6& xi); - - /** - * wedge for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = (wx,wy,wz) 3D angular velocity - * v (vx,vy,vz) = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { - return (Matrix(4,4) << - 0.,-wz, wy, vx, - wz, 0.,-wx, vy, - -wy, wx, 0., vz, - 0., 0., 0., 0.).finished(); - } - - /// @} - /// @name Group Action on Point3 - /// @{ - - /** - * @brief takes point in Pose coordinates and transforms it to world coordinates - * @param p point in Pose coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point - * @return point in world coordinates - */ - Point3 transform_from(const Point3& p, - OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const; - - /** syntactic sugar for transform_from */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } - - /** - * @brief takes point in world coordinates and transforms it to Pose coordinates - * @param p point in world coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point - * @return point in Pose coordinates - */ - Point3 transform_to(const Point3& p, - OptionalJacobian<3,6> Dpose = boost::none, - OptionalJacobian<3,3> Dpoint = boost::none) const; - - /// @} - /// @name Standard Interface - /// @{ - - /// get rotation - const Rot3& rotation() const { return R_; } - - /// get translation - const Point3& translation() const { return t_; } - - /// get x - double x() const { return t_.x(); } - - /// get y - double y() const { return t_.y(); } - - /// get z - double z() const { return t_.z(); } - - /** convert to 4*4 matrix */ - Matrix4 matrix() const; - - /** receives a pose in world coordinates and transforms it to local coordinates */ - Pose3 transform_to(const Pose3& pose) const; - - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @return range (double) - */ - double range(const Point3& point, - OptionalJacobian<1,6> H1=boost::none, - OptionalJacobian<1,3> H2=boost::none) const; - - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @return range (double) - */ - double range(const Pose3& pose, - OptionalJacobian<1,6> H1=boost::none, - OptionalJacobian<1,6> H2=boost::none) const; - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Return the start and end indices (inclusive) of the translation component of the - * exponential map parameterization - * @return a pair of [start, end] indices into the tangent space vector - */ - inline static std::pair translationInterval() { return std::make_pair(3, 5); } - - /** - * Return the start and end indices (inclusive) of the rotation component of the - * exponential map parameterization - * @return a pair of [start, end] indices into the tangent space vector - */ - static std::pair rotationInterval() { return std::make_pair(0, 2); } - - /// Output stream operator - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(R_); - ar & BOOST_SERIALIZATION_NVP(t_); - } - /// @} - - };// Pose3 class + using LieGroup::inverse; // version with derivative /** * wedge for Pose3: * @param xi 6-dim twist (omega,v) where - * omega = 3D angular velocity - * v = 3D velocity + * omega = (wx,wy,wz) 3D angular velocity + * v (vx,vy,vz) = 3D velocity * @return xihat, 4*4 element of Lie algebra that can be exponentiated */ + static Matrix wedge(double wx, double wy, double wz, double vx, double vy, + double vz) { + return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished(); + } + + /// @} + /// @name Group Action on Point3 + /// @{ + + /** + * @brief takes point in Pose coordinates and transforms it to world coordinates + * @param p point in Pose coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in world coordinates + */ + Point3 transform_from(const Point3& p, OptionalJacobian<3, 6> Dpose = + boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; + + /** syntactic sugar for transform_from */ + inline Point3 operator*(const Point3& p) const { + return transform_from(p); + } + + /** + * @brief takes point in world coordinates and transforms it to Pose coordinates + * @param p point in world coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in Pose coordinates + */ + Point3 transform_to(const Point3& p, OptionalJacobian<3, 6> Dpose = + boost::none, OptionalJacobian<3, 3> Dpoint = boost::none) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// get rotation + const Rot3& rotation() const { + return R_; + } + + /// get translation + const Point3& translation() const { + return t_; + } + + /// get x + double x() const { + return t_.x(); + } + + /// get y + double y() const { + return t_.y(); + } + + /// get z + double z() const { + return t_.z(); + } + + /** convert to 4*4 matrix */ + Matrix4 matrix() const; + + /** receives a pose in world coordinates and transforms it to local coordinates */ + Pose3 transform_to(const Pose3& pose) const; + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @return range (double) + */ + double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const; + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @return range (double) + */ + double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 6> H2 = boost::none) const; + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair translationInterval() { + return std::make_pair(3, 5); + } + + /** + * Return the start and end indices (inclusive) of the rotation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + static std::pair rotationInterval() { + return std::make_pair(0, 2); + } + + /// Output stream operator + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const Pose3& p); + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + } + /// @} + +}; +// Pose3 class + +/** + * wedge for Pose3: + * @param xi 6-dim twist (omega,v) where + * omega = 3D angular velocity + * v = 3D velocity + * @return xihat, 4*4 element of Lie algebra that can be exponentiated + */ template<> inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); @@ -350,21 +322,8 @@ inline Matrix wedge(const Vector& xi) { typedef std::pair Point3Pair; GTSAM_EXPORT boost::optional align(const std::vector& pairs); -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_group : public boost::true_type{ +struct traits : public internal::LieGroupTraits { }; -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -} - } // namespace gtsam diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h new file mode 100644 index 000000000..534b4ab42 --- /dev/null +++ b/gtsam/geometry/Quaternion.h @@ -0,0 +1,167 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Quaternion.h + * @brief Lie Group wrapper for Eigen Quaternions + * @author Frank Dellaert + **/ + +#include +#include + +#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> + +namespace gtsam { + +// Define traits +template +struct traits { + typedef QUATERNION_TYPE ManifoldType; + typedef QUATERNION_TYPE Q; + + typedef lie_group_tag structure_category; + typedef multiplicative_group_tag group_flavor; + + /// @name Group traits + /// @{ + static Q Identity() { + return Q::Identity(); + } + + static Q Compose(const Q &g, const Q & h) { + return g * h; + } + + static Q Between(const Q &g, const Q & h) { + Q d = g.inverse() * h; + return d; + } + + static Q Inverse(const Q &g) { + return g.inverse(); + } + + /// @} + /// @name Basic manifold traits + /// @{ + enum { + dimension = 3 + }; + typedef OptionalJacobian<3, 3> ChartJacobian; + typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; + + /// @} + /// @name Lie group traits + /// @{ + static Q Compose(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh = + boost::none) { + if (Hg) + *Hg = h.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart ( h.toRotationMatrix().transpose() ? ) + if (Hh) + *Hh = I_3x3; // TODO : check Jacobian consistent with chart ( I(3)? ) + return g * h; + } + + static Q Between(const Q &g, const Q & h, ChartJacobian Hg, ChartJacobian Hh = + boost::none) { + Q d = g.inverse() * h; + if (Hg) + *Hg = -d.toRotationMatrix().transpose(); // TODO : check Jacobian consistent with chart + if (Hh) + *Hh = I_3x3; // TODO : check Jacobian consistent with chart (my guess I(3) ) + return d; + } + + static Q Inverse(const Q &g, ChartJacobian H) { + if (H) + *H = -g.toRotationMatrix(); // TODO : check Jacobian consistent with chart + return g.inverse(); + } + + /// Exponential map, simply be converting omega to axis/angle representation + static Q Expmap(const Eigen::Ref& omega, + ChartJacobian H = boost::none) { + if (omega.isZero()) + return Q::Identity(); + else { + _Scalar angle = omega.norm(); + return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle)); + } + if (H) CONCEPT_NOT_IMPLEMENTED; + } + + /// We use our own Logmap, as there is a slight bug in Eigen + static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) { + using std::acos; + using std::sqrt; + + // define these compile time constants to avoid std::abs: + static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, + NearlyNegativeOne = -1.0 + 1e-10; + + const double qw = q.w(); + if (qw > NearlyOne) { + // Taylor expansion of (angle / s) at 1 + //return (2 + 2 * (1-qw) / 3) * q.vec(); + return (8. / 3. - 2. / 3. * qw) * q.vec(); + } else if (qw < NearlyNegativeOne) { + // Taylor expansion of (angle / s) at -1 + //return (-2 - 2 * (1 + qw) / 3) * q.vec(); + return (-8. / 3 + 2. / 3 * qw) * q.vec(); + } else { + // Normal, away from zero case + double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); + // Important: convert to [-pi,pi] to keep error continuous + if (angle > M_PI) + angle -= twoPi; + else if (angle < -M_PI) + angle += twoPi; + return (angle / s) * q.vec(); + } + + if (H) CONCEPT_NOT_IMPLEMENTED; + } + + /// @} + /// @name Manifold traits + /// @{ + static TangentVector Local(const Q& origin, const Q& other, + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { + return Logmap(Between(origin, other, Horigin, Hother)); + // TODO: incorporate Jacobian of Logmap + } + static Q Retract(const Q& origin, const TangentVector& v, + ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { + return Compose(origin, Expmap(v), Horigin, Hv); + // TODO : incorporate Jacobian of Expmap + } + + /// @} + /// @name Testable + /// @{ + static void Print(const Q& q, const std::string& str = "") { + if (str.size() == 0) + std::cout << "Eigen::Quaternion: "; + else + std::cout << str << " "; + std::cout << q.vec().transpose() << std::endl; + } + static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) { + return Between(q1, q2).vec().array().abs().maxCoeff() < tol; + } + /// @} +}; + +typedef Eigen::Quaternion Quaternion; + +} // \namespace gtsam + diff --git a/gtsam/geometry/Rot2.cpp b/gtsam/geometry/Rot2.cpp index 13c22ddc1..41d56b6e3 100644 --- a/gtsam/geometry/Rot2.cpp +++ b/gtsam/geometry/Rot2.cpp @@ -17,15 +17,11 @@ */ #include -#include using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Rot2); - /* ************************************************************************* */ Rot2 Rot2::fromCosSin(double c, double s) { if (fabs(c * c + s * s - 1.0) > 1e-9) { @@ -63,6 +59,24 @@ Rot2& Rot2::normalize() { return *this; } +/* ************************************************************************* */ +Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) { + if (H) + *H = I_1x1; + if (zero(v)) + return (Rot2()); + else + return Rot2::fromAngle(v(0)); +} + +/* ************************************************************************* */ +Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) { + if (H) + *H = I_1x1; + Vector1 v; + v << r.theta(); + return v; +} /* ************************************************************************* */ Matrix2 Rot2::matrix() const { Matrix2 rvalue_; diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 231522102..95f025622 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -18,10 +18,9 @@ #pragma once -#include - -#include #include +#include +#include namespace gtsam { @@ -31,25 +30,16 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot2 { - - public: - /** get the dimension by the type */ - static const size_t dimension = 1; - - private: + class GTSAM_EXPORT Rot2 : public LieGroup { /** we store cos(theta) and sin(theta) */ double c_, s_; - /** normalize to make sure cos and sin form unit vector */ Rot2& normalize(); /** private constructor from cos/sin */ - inline Rot2(double c, double s) : - c_(c), s_(s) { - } + inline Rot2(double c, double s) : c_(c), s_(s) {} public: @@ -57,14 +47,10 @@ namespace gtsam { /// @{ /** default constructor, zero rotation */ - Rot2() : - c_(1.0), s_(0.0) { - } + Rot2() : c_(1.0), s_(0.0) {} /// Constructor from angle in radians == exponential map at identity - Rot2(double theta) : - c_(cos(theta)), s_(sin(theta)) { - } + Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {} /// Named constructor from angle in radians static Rot2 fromAngle(double theta) { @@ -111,69 +97,25 @@ namespace gtsam { inline static Rot2 identity() { return Rot2(); } /** The inverse rotation - negative angle */ - Rot2 inverse() const { - return Rot2(c_, -s_); - } - - /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 = - boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 = I_1x1; // set to 1x1 identity matrix - if (H2) *H2 = I_1x1; // set to 1x1 identity matrix - return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); - } + Rot2 inverse() const { return Rot2(c_, -s_);} /** Compose - make a new rotation by adding angles */ Rot2 operator*(const Rot2& R) const { return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } - /** Between using the default implementation */ - inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 = - boost::none, OptionalJacobian<1,1> H2 = boost::none) const { - if (H1) *H1 = -I_1x1; // set to 1x1 identity matrix - if (H2) *H2 = I_1x1; // set to 1x1 identity matrix - return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); - } - - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { - return dimension; - } - - /// Dimensionality of the tangent space, DOF = 1 - inline size_t dim() const { - return dimension; - } - - /// Updates a with tangent space delta - inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } - - /// Returns inverse retraction - inline Vector1 localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } - /// @} /// @name Lie Group /// @{ - ///Exponential map at identity - create a rotation from canonical coordinates - static Rot2 Expmap(const Vector& v) { - if (zero(v)) - return (Rot2()); - else - return Rot2::fromAngle(v(0)); - } + /// Exponential map at identity - create a rotation from canonical coordinates + static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none); - ///Log map at identity - return the canonical coordinates of this rotation - static inline Vector1 Logmap(const Rot2& r) { - Vector1 v; - v << r.theta(); - return v; - } + /// Log map at identity - return the canonical coordinates of this rotation + static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none); + + /** Calculate Adjoint map */ + Matrix1 AdjointMap() const { return I_1x1; } /// Left-trivialized derivative of the exponential map static Matrix ExpmapDerivative(const Vector& v) { @@ -185,6 +127,18 @@ namespace gtsam { return ones(1); } + // Chart at origin simply uses exponential map and its inverse + struct ChartAtOrigin { + static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) { + return Expmap(v, H); + } + static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) { + return Logmap(r, H); + } + }; + + using LieGroup::inverse; // version with derivative + /// @} /// @name Group Action on Point2 /// @{ @@ -253,20 +207,7 @@ namespace gtsam { }; - // Define GTSAM traits - namespace traits { - template<> - struct GTSAM_EXPORT is_group : public boost::true_type{ - }; + struct traits : public internal::LieGroupTraits {}; - template<> - struct GTSAM_EXPORT is_manifold : public boost::true_type{ - }; - - template<> - struct GTSAM_EXPORT dimension : public boost::integral_constant{ - }; - - } } // gtsam diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index f494df07b..ab44716c8 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -234,8 +234,8 @@ ostream &operator<<(ostream &os, const Rot3& R) { Rot3 Rot3::slerp(double t, const Rot3& other) const { // amazingly simple in GTSAM :-) assert(t>=0 && t<=1); - Vector3 omega = localCoordinates(other, Rot3::EXPMAP); - return retract(t * omega, Rot3::EXPMAP); + Vector3 omega = Logmap(between(other)); + return compose(Expmap(t * omega)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 630dad356..e9568fb26 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -22,6 +22,9 @@ #pragma once +#include +#include +#include #include // Get GTSAM_USE_QUATERNIONS macro // You can override the default coordinate mode using this flag @@ -40,18 +43,8 @@ #endif #endif -#include -#include -#include -#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; - /** * @brief A 3D rotation represented as a rotation matrix if the preprocessor * symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it @@ -59,7 +52,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot3 { + class GTSAM_EXPORT Rot3 : public LieGroup { private: @@ -161,7 +154,7 @@ namespace gtsam { * @param theta rotation angle * @return incremental rotation matrix */ - static Rot3 rodriguez(const Vector& w, double theta); + static Rot3 rodriguez(const Vector3& w, double theta); /** * Rodriguez' formula to compute an incremental rotation matrix @@ -215,16 +208,13 @@ namespace gtsam { return Rot3(); } - /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; - /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; + Rot3 inverse() const { + return Rot3(Matrix3(transpose())); + } + /** * Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C * @param cRb rotation from B frame to C frame @@ -235,23 +225,10 @@ namespace gtsam { return cRb * (*this) * cRb.inverse(); } - /** - * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' - */ - Rot3 between(const Rot3& R2, - OptionalJacobian<3,3> H1=boost::none, - OptionalJacobian<3,3> H2=boost::none) const; - /// @} /// @name Manifold /// @{ - /// dimension of the variable - used to autodetect sizes - static size_t Dim() { return 3; } - - /// return dimensionality of tangent space, DOF = 3 - size_t dim() const { return 3; } - /** * The method retract() is used to map from the tangent space back to the manifold. * Its inverse, is localCoordinates(). For Lie groups, an obvious retraction is the @@ -265,21 +242,29 @@ namespace gtsam { EXPMAP, ///< Use the Lie group exponential map to retract #ifndef GTSAM_USE_QUATERNIONS CAYLEY, ///< Retract and localCoordinates using the Cayley transform. - SLOW_CAYLEY ///< Slow matrix implementation of Cayley transform (for tests only). #endif }; #ifndef GTSAM_USE_QUATERNIONS + + // Cayley chart around origin + struct CayleyChart { + static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none); + static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none); + }; + /// Retraction from R^3 to Rot3 manifold using the Cayley transform - Rot3 retractCayley(const Vector& omega) const; + Rot3 retractCayley(const Vector& omega) const { + return compose(CayleyChart::Retract(omega)); + } + + /// Inverse of retractCayley + Vector3 localCayley(const Rot3& other) const { + return CayleyChart::Local(between(other)); + } + #endif - /// Retraction from R^3 \f$ [R_x,R_y,R_z] \f$ to Rot3 manifold neighborhood around current rotation - Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; - - /// Returns local retract coordinates \f$ [R_x,R_y,R_z] \f$ in neighborhood around current rotation - Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; - /// @} /// @name Lie Group /// @{ @@ -305,6 +290,17 @@ namespace gtsam { /// Derivative of Logmap static Matrix3 LogmapDerivative(const Vector3& x); + /** Calculate Adjoint map */ + Matrix3 AdjointMap() const { return matrix(); } + + // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE + struct ChartAtOrigin { + static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none); + static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none); + }; + + using LieGroup::inverse; // version with derivative + /// @} /// @name Group Action on Point3 /// @{ @@ -465,20 +461,7 @@ namespace gtsam { */ GTSAM_EXPORT std::pair RQ(const Matrix3& A); - // Define GTSAM traits - namespace traits { - template<> - struct GTSAM_EXPORT is_group : public boost::true_type{ - }; - - template<> - struct GTSAM_EXPORT is_manifold : public boost::true_type{ - }; - - template<> - struct GTSAM_EXPORT dimension : public boost::integral_constant{ - }; - - } + struct traits : public internal::LieGroupTraits {}; } + diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 041a1b854..aa2f60de9 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -117,7 +117,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { } /* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w, double theta) { +Rot3 Rot3::rodriguez(const Vector3& w, double theta) { // get components of axis \omega double wx = w(0), wy=w(1), wz=w(2); double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; @@ -139,13 +139,6 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { -swy + C02, swx + C12, c + C22); } -/* ************************************************************************* */ -Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I_3x3; - return *this * R2; -} - /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(Matrix3(rot_*R2.rot_)); @@ -157,21 +150,6 @@ Matrix3 Rot3::transpose() const { return rot_.transpose(); } -/* ************************************************************************* */ -Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { - if (H1) *H1 = -rot_; - return Rot3(Matrix3(transpose())); -} - -/* ************************************************************************* */ -Rot3 Rot3::between (const Rot3& R2, - OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I_3x3; - Matrix3 R12 = transpose()*R2.rot_; - return Rot3(R12); -} - /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { @@ -228,60 +206,49 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { } /* ************************************************************************* */ -Rot3 Rot3::retractCayley(const Vector& omega) const { +Rot3 Rot3::CayleyChart::Retract(const Vector3& omega, OptionalJacobian<3,3> H) { + if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative"); const double x = omega(0), y = omega(1), z = omega(2); const double x2 = x * x, y2 = y * y, z2 = z * z; const double xy = x * y, xz = x * z, yz = y * z; const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; - return (*this) - * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, + return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); } /* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return (*this)*Expmap(omega); - } else if(mode == Rot3::CAYLEY) { - return retractCayley(omega); - } else if(mode == Rot3::SLOW_CAYLEY) { - Matrix3 Omega = skewSymmetric(omega); - return (*this)*CayleyFixed<3>(-Omega/2); - } else { - assert(false); - exit(1); - } +Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { + if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); + // Create a fixed-size matrix + Matrix3 A = R.matrix(); + // Mathematica closed form optimization (procrastination?) gone wild: + const double a = A(0, 0), b = A(0, 1), c = A(0, 2); + const double d = A(1, 0), e = A(1, 1), f = A(1, 2); + const double g = A(2, 0), h = A(2, 1), i = A(2, 2); + const double di = d * i, ce = c * e, cd = c * d, fg = f * g; + const double M = 1 + e - f * h + i + e * i; + const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); } /* ************************************************************************* */ -Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return Logmap(between(T)); - } else if(mode == Rot3::CAYLEY) { - // Create a fixed-size matrix - Matrix3 A = rot_.transpose() * T.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a=A(0,0),b=A(0,1),c=A(0,2); - const double d=A(1,0),e=A(1,1),f=A(1,2); - const double g=A(2,0),h=A(2,1),i=A(2,2); - const double di = d*i, ce = c*e, cd = c*d, fg=f*g; - const double M = 1 + e - f*h + i + e*i; - const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = a * f - cd + f; - const double y = b * f - ce - c; - const double z = fg - di - d; - return K * Vector3(x, y, z); - } else if(mode == Rot3::SLOW_CAYLEY) { - // Create a fixed-size matrix - Matrix3 A(between(T).matrix()); - // using templated version of Cayley - Matrix3 Omega = CayleyFixed<3>(A); - return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); - } else { - assert(false); - exit(1); - } +Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Expmap(omega, H); + if (mode == Rot3::CAYLEY) return CayleyChart::Retract(omega, H); + else throw std::runtime_error("Rot3::Retract: unknown mode"); +} + +/* ************************************************************************* */ +Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Logmap(R, H); + if (mode == Rot3::CAYLEY) return CayleyChart::Local(R, H); + else throw std::runtime_error("Rot3::Local: unknown mode"); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 5c2158ab4..8696900aa 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -84,8 +84,9 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::rodriguez(const Vector& w, double theta) { - return Quaternion(Eigen::AngleAxisd(theta, w)); } + Rot3 Rot3::rodriguez(const Vector3& w, double theta) { + return QuaternionChart::Expmap(theta,w); + } /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, @@ -119,7 +120,7 @@ namespace gtsam { OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = -(R2.transpose()*matrix()); if (H2) *H2 = I3; - return between_default(*this, R2); + return inverse() * R2; } /* ************************************************************************* */ @@ -133,35 +134,9 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { - using std::acos; - using std::sqrt; - static const double twoPi = 2.0 * M_PI, - // define these compile time constants to avoid std::abs: - NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; - - Vector3 thetaR; - const Quaternion& q = R.quaternion_; - const double qw = q.w(); - if (qw > NearlyOne) { - // Taylor expansion of (angle / s) at 1 - thetaR = (2 - 2 * (qw - 1) / 3) * q.vec(); - } else if (qw < NearlyNegativeOne) { - // Angle is zero, return zero vector - thetaR = Vector3::Zero(); - } else { - // Normal, away from zero case - double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); - // Important: convert to [-pi,pi] to keep error continuous - if (angle > M_PI) - angle -= twoPi; - else if (angle < -M_PI) - angle += twoPi; - thetaR = (angle / s) * q.vec(); - } - + Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { if(H) *H = Rot3::LogmapDerivative(thetaR); - return thetaR; + return QuaternionChart::Logmap(R.quaternion_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp new file mode 100644 index 000000000..08ae31945 --- /dev/null +++ b/gtsam/geometry/SO3.cpp @@ -0,0 +1,104 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SO3.cpp + * @brief 3*3 matrix representation o SO(3) + * @author Frank Dellaert + * @date December 2014 + */ + +#include +#include +#include + +namespace gtsam { + +SO3 Rodrigues(const double& theta, const Vector3& axis) { + using std::cos; + using std::sin; + + // get components of axis \omega + double wx = axis(0), wy = axis(1), wz = axis(2); + + double c = cos(theta), s = sin(theta), c_1 = 1 - c; + double wwTxx = wx * wx, wwTyy = wy * wy, wwTzz = wz * wz; + double swx = wx * s, swy = wy * s, swz = wz * s; + + double C00 = c_1 * wwTxx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz; + double C11 = c_1 * wwTyy, C12 = c_1 * wy * wz; + double C22 = c_1 * wwTzz; + + Matrix3 R; + R << c + C00, -swz + C01, swy + C02, // + swz + C01, c + C11, -swx + C12, // + -swy + C02, swx + C12, c + C22; + + return R; +} + +/// simply convert omega to axis/angle representation +SO3 SO3::Expmap(const Eigen::Ref& omega, + ChartJacobian H) { + + if (H) + CONCEPT_NOT_IMPLEMENTED; + + if (omega.isZero()) + return SO3::Identity(); + else { + double angle = omega.norm(); + return Rodrigues(angle, omega / angle); + } +} + +Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { + using std::sqrt; + using std::sin; + + if (H) + CONCEPT_NOT_IMPLEMENTED; + + // note switch to base 1 + const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2); + const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2); + const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); + + // Get trace(R) + double tr = R.trace(); + + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. + // we do something special + if (std::abs(tr + 1.0) < 1e-10) { + if (std::abs(R33 + 1.0) > 1e-10) + return (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); + else if (std::abs(R22 + 1.0) > 1e-10) + return (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); + else + // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + return (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); + } else { + double magnitude; + double tr_3 = tr - 3.0; // always negative + if (tr_3 < -1e-7) { + double theta = acos((tr - 1.0) / 2.0); + magnitude = theta / (2.0 * sin(theta)); + } else { + // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) + // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) + magnitude = 0.5 - tr_3 * tr_3 / 12.0; + } + return magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); + } +} + +} // end namespace gtsam + diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h new file mode 100644 index 000000000..bed2f1212 --- /dev/null +++ b/gtsam/geometry/SO3.h @@ -0,0 +1,90 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SO3.h + * @brief 3*3 matrix representation of SO(3) + * @author Frank Dellaert + * @date December 2014 + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * True SO(3), i.e., 3*3 matrix subgroup + * We guarantee (all but first) constructors only generate from sub-manifold. + * However, round-off errors in repeated composition could move off it... + */ +class SO3: public Matrix3, public LieGroup { + +protected: + +public: + enum { dimension=3 }; + + /// Constructor from AngleAxisd + SO3() : Matrix3(I_3x3) { + } + + /// Constructor from Eigen Matrix + template + SO3(const MatrixBase& R) : + Matrix3(R.eval()) { + } + + /// Constructor from AngleAxisd + SO3(const Eigen::AngleAxisd& angleAxis) : + Matrix3(angleAxis) { + } + + void print(const std::string& s) const { + std::cout << s << *this << std::endl; + } + + bool equals(const SO3 & R, double tol) const { + return equal_with_abs_tol(*this, R, tol); + } + + static SO3 identity() { return I_3x3; } + SO3 inverse() const { return this->Matrix3::inverse(); } + + static SO3 Expmap(const Eigen::Ref& omega, ChartJacobian H = boost::none); + static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); + + Matrix3 AdjointMap() const { return *this; } + + // Chart at origin + struct ChartAtOrigin { + static SO3 Retract(const Vector3& v, ChartJacobian H = boost::none) { + return Expmap(v,H); + } + static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) { + return Logmap(R,H); + } + }; + + using LieGroup::inverse; + +}; + +template<> +struct traits : public internal::LieGroupTraits {}; + + +} // end namespace gtsam + diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 7650aa086..49abcf36b 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -44,6 +44,8 @@ private: public: + enum { dimension = 6 }; + /// @name Standard Constructors /// @{ @@ -144,22 +146,7 @@ private: }; -// Define GTSAM traits -namespace traits { - template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static StereoCamera value() { return StereoCamera();} -}; - -} +struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 694bf525b..2ec745fd8 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -121,7 +121,7 @@ namespace gtsam { /** The difference between another point and this point */ inline StereoPoint2 between(const StereoPoint2& p2) const { - return gtsam::between_default(*this, p2); + return p2 - *this; } /// @} @@ -174,20 +174,6 @@ namespace gtsam { }; - // Define GTSAM traits - namespace traits { - template<> - struct is_group : public boost::true_type { - }; - - template<> - struct is_manifold : public boost::true_type { - }; - - template<> - struct dimension : public boost::integral_constant { - }; - - } + struct traits : public internal::Manifold {}; } diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 5b4a8b33e..611cf7cde 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -131,7 +131,7 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { Unit3 Unit3::retract(const Vector2& v) const { // Get the vector form of the point and the basis matrix - Vector3 p = Point3::Logmap(p_); + Vector3 p = p_.vector(); Matrix32 B = basis(); // Compute the 3D xi_hat vector @@ -156,8 +156,7 @@ Unit3 Unit3::retract(const Vector2& v) const { /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& y) const { - Vector3 p = Point3::Logmap(p_); - Vector3 q = Point3::Logmap(y.p_); + Vector3 p = p_.vector(), q = y.p_.vector(); double dot = p.dot(q); // Check for special cases diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index f83404e74..79c3977cd 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include #include @@ -28,7 +29,7 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class GTSAM_EXPORT Unit3{ +class GTSAM_EXPORT Unit3 { private: @@ -37,6 +38,8 @@ private: public: + enum { dimension = 2 }; + /// @name Constructors /// @{ @@ -160,24 +163,7 @@ private: }; // Define GTSAM traits -namespace traits { - -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -template<> -struct GTSAM_EXPORT zero { - static Unit3 value() { - return Unit3(); - } -}; - -} +template <> struct traits : public internal::Manifold {}; } // namespace gtsam diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp new file mode 100644 index 000000000..a15d7e2c2 --- /dev/null +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -0,0 +1,79 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testCyclic.cpp + * @brief Unit tests for cyclic group + * @author Frank Dellaert + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef Cyclic<3> G; // Let's use the cyclic group of order 3 + +//****************************************************************************** +TEST(Cyclic, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + EXPECT_LONGS_EQUAL(0, traits::Identity()); +} + +//****************************************************************************** +TEST(Cyclic, Constructor) { + G g(0); +} + +//****************************************************************************** +TEST(Cyclic, Compose) { + EXPECT_LONGS_EQUAL(0, traits::Compose(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, traits::Compose(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, traits::Compose(G(0),G(2))); + + EXPECT_LONGS_EQUAL(2, traits::Compose(G(2),G(0))); + EXPECT_LONGS_EQUAL(0, traits::Compose(G(2),G(1))); + EXPECT_LONGS_EQUAL(1, traits::Compose(G(2),G(2))); +} + +//****************************************************************************** +TEST(Cyclic, Between) { + EXPECT_LONGS_EQUAL(0, traits::Between(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, traits::Between(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, traits::Between(G(0),G(2))); + + EXPECT_LONGS_EQUAL(1, traits::Between(G(2),G(0))); + EXPECT_LONGS_EQUAL(2, traits::Between(G(2),G(1))); + EXPECT_LONGS_EQUAL(0, traits::Between(G(2),G(2))); +} + +//****************************************************************************** +TEST(Cyclic, Ivnverse) { + EXPECT_LONGS_EQUAL(0, traits::Inverse(G(0))); + EXPECT_LONGS_EQUAL(2, traits::Inverse(G(1))); + EXPECT_LONGS_EQUAL(1, traits::Inverse(G(2))); +} + +//****************************************************************************** +TEST(Cyclic , Invariants) { + G g(2), h(1); + check_group_invariants(g,h); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index ab40928ab..fce7955e9 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -27,6 +27,34 @@ using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Point2) GTSAM_CONCEPT_LIE_INST(Point2) +//****************************************************************************** +TEST(Double , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** +TEST(Double , Invariants) { + double p1(2), p2(5); + check_group_invariants(p1, p2); + check_manifold_invariants(p1, p2); +} + +//****************************************************************************** +TEST(Point2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** +TEST(Point2 , Invariants) { + Point2 p1(1, 2), p2(4, 5); + check_group_invariants(p1, p2); + check_manifold_invariants(p1, p2); +} + /* ************************************************************************* */ TEST(Point2, constructor) { Point2 p1(1, 2), p2 = p1; @@ -38,16 +66,16 @@ TEST(Point2, Lie) { Point2 p1(1, 2), p2(4, 5); Matrix H1, H2; - EXPECT(assert_equal(Point2(5,7), p1.compose(p2, H1, H2))); + EXPECT(assert_equal(Point2(5,7), traits::Compose(p1, p2, H1, H2))); EXPECT(assert_equal(eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(3,3), p1.between(p2, H1, H2))); + EXPECT(assert_equal(Point2(3,3), traits::Between(p1, p2, H1, H2))); EXPECT(assert_equal(-eye(2), H1)); EXPECT(assert_equal(eye(2), H2)); - EXPECT(assert_equal(Point2(5,7), p1.retract(Vector2(4., 5.)))); - EXPECT(assert_equal(Vector2(3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point2(5,7), traits::Retract(p1, Vector2(4., 5.)))); + EXPECT(assert_equal(Vector2(3.,3.), traits::Local(p1,p2))); } /* ************************************************************************* */ @@ -55,7 +83,7 @@ TEST( Point2, expmap) { Vector d(2); d(0) = 1; d(1) = -1; - Point2 a(4, 5), b = a.retract(d), c(5, 4); + Point2 a(4, 5), b = traits::Retract(a,d), c(5, 4); EXPECT(assert_equal(b,c)); } @@ -108,6 +136,10 @@ TEST( Point2, norm ) { EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9); expectedH = numericalDerivative11(norm_proxy, x2); EXPECT(assert_equal(expectedH,actualH)); + + // analytical + expectedH = (Matrix(1, 2) << x2.x()/actual, x2.y()/actual).finished(); + EXPECT(assert_equal(expectedH,actualH)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 18273b182..5c6b32dca 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -26,22 +26,36 @@ GTSAM_CONCEPT_LIE_INST(Point3) static Point3 P(0.2, 0.7, -2); +//****************************************************************************** +TEST(Point3 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** +TEST(Point3 , Invariants) { + Point3 p1(1, 2, 3), p2(4, 5, 6); + check_group_invariants(p1, p2); + check_manifold_invariants(p1, p2); +} + /* ************************************************************************* */ TEST(Point3, Lie) { Point3 p1(1, 2, 3); Point3 p2(4, 5, 6); Matrix H1, H2; - EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2))); + EXPECT(assert_equal(Point3(5, 7, 9), traits::Compose(p1, p2, H1, H2))); EXPECT(assert_equal(eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2))); + EXPECT(assert_equal(Point3(3, 3, 3), traits::Between(p1, p2, H1, H2))); EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5, 7, 9), p1.retract(Vector3(4., 5., 6.)))); - EXPECT(assert_equal((Vector)Vector3(3.,3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point3(5, 7, 9), traits::Retract(p1, Vector3(4,5,6)))); + EXPECT(assert_equal(Vector3(3, 3, 3), traits::Local(p1,p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 5d3f52516..fa6bd203c 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include @@ -32,11 +32,16 @@ using namespace boost::assign; using namespace gtsam; using namespace std; -//#define SLOW_BUT_CORRECT_EXPMAP - GTSAM_CONCEPT_TESTABLE_INST(Pose2) GTSAM_CONCEPT_LIE_INST(Pose2) +//****************************************************************************** +TEST(Pose2 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + /* ************************************************************************* */ TEST(Pose2, constructors) { Point2 p; @@ -138,7 +143,6 @@ TEST(Pose2, expmap0d) { EXPECT(assert_equal(expected, actual, 1e-5)); } -#ifdef SLOW_BUT_CORRECT_EXPMAP /* ************************************************************************* */ // test case for screw motion in the plane namespace screw { @@ -149,13 +153,12 @@ namespace screw { Pose2 expected(expectedR, expectedT); } -TEST(Pose3, expmap_c) +TEST(Pose2, expmap_c) { EXPECT(assert_equal(screw::expected, expm(screw::xi),1e-6)); EXPECT(assert_equal(screw::expected, Pose2::Expmap(screw::xi),1e-6)); EXPECT(assert_equal(screw::xi, Pose2::Logmap(screw::expected),1e-6)); } -#endif /* ************************************************************************* */ TEST(Pose2, expmap_c_full) @@ -175,9 +178,9 @@ TEST(Pose2, logmap) { Pose2 pose0(M_PI/2.0, Point2(1, 2)); Pose2 pose(M_PI/2.0+0.018, Point2(1.015, 2.01)); #ifdef SLOW_BUT_CORRECT_EXPMAP - Vector expected = Vector3(0.00986473, -0.0150896, 0.018); + Vector3 expected(0.00986473, -0.0150896, 0.018); #else - Vector expected = Vector3(0.01, -0.015, 0.018); + Vector3 expected(0.01, -0.015, 0.018); #endif Vector actual = pose0.localCoordinates(pose); EXPECT(assert_equal(expected, actual, 1e-5)); @@ -193,34 +196,45 @@ TEST(Pose2, logmap_full) { } /* ************************************************************************* */ -Vector3 w = (Vector(3) << 0.1, 0.27, -0.3).finished(); -Vector3 w0 = (Vector(3) << 0.1, 0.27, 0.0).finished(); // alpha = 0 - -// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? -// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector3 testDexpL(const Vector3 w, const Vector3& dw) { - Vector3 y = Pose2::Logmap(Pose2::Expmap(-w) * Pose2::Expmap(w + dw)); - return y; +TEST( Pose2, ExpmapDerivative1) { + Matrix3 actualH; + Vector3 w(0.1, 0.27, -0.3); + Pose2::Expmap(w,actualH); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } -TEST( Pose2, ExpmapDerivative) { - Matrix actualDexpL = Pose2::ExpmapDerivative(w); - Matrix expectedDexpL = numericalDerivative11( - boost::bind(testDexpL, w, _1), zero(3), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); +/* ************************************************************************* */ +TEST( Pose2, ExpmapDerivative2) { + Matrix3 actualH; + Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 + Pose2::Expmap(w0,actualH); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Expmap, w0, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} - Matrix actualDexpInvL = Pose2::LogmapDerivative(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); +/* ************************************************************************* */ +TEST( Pose2, LogmapDerivative1) { + Matrix3 actualH; + Vector3 w(0.1, 0.27, -0.3); + Pose2 p = Pose2::Expmap(w); + EXPECT(assert_equal(w, Pose2::Logmap(p,actualH), 1e-5)); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} - // test case where alpha = 0 - Matrix actualDexpL0 = Pose2::ExpmapDerivative(w0); - Matrix expectedDexpL0 = numericalDerivative11( - boost::bind(testDexpL, w0, _1), zero(3), 1e-2); - EXPECT(assert_equal(expectedDexpL0, actualDexpL0, 1e-5)); - - Matrix actualDexpInvL0 = Pose2::LogmapDerivative(w0); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); +/* ************************************************************************* */ +TEST( Pose2, LogmapDerivative2) { + Matrix3 actualH; + Vector3 w0(0.1, 0.27, 0.0); // alpha = 0 + Pose2 p = Pose2::Expmap(w0); + EXPECT(assert_equal(w0, Pose2::Logmap(p,actualH), 1e-5)); + Matrix3 expectedH = numericalDerivative21 >(&Pose2::Logmap, p, boost::none, 1e-2); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); } /* ************************************************************************* */ @@ -513,13 +527,6 @@ TEST( Pose2, round_trip ) EXPECT(assert_equal(odo, p1.between(p2))); } -/* ************************************************************************* */ -TEST(Pose2, members) -{ - Pose2 pose; - EXPECT(pose.dim() == 3); -} - namespace { /* ************************************************************************* */ // some shared test values @@ -760,6 +767,47 @@ TEST(Pose2, align_4) { EXPECT(assert_equal(expected, *actual)); } +//****************************************************************************** +Pose2 T1(M_PI / 4.0, Point2(sqrt(0.5), sqrt(0.5))); +Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0)); + +//****************************************************************************** +TEST(Pose2 , Invariants) { + Pose2 id; + + check_group_invariants(id,id); + check_group_invariants(id,T1); + check_group_invariants(T2,id); + check_group_invariants(T2,T1); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,T1); + check_manifold_invariants(T2,id); + check_manifold_invariants(T2,T1); + +} + +//****************************************************************************** +TEST(Pose2 , LieGroupDerivatives) { + Pose2 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T1); + +} + +//****************************************************************************** +TEST(Pose2 , ChartDerivatives) { + Pose2 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T1); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 421dea879..d40344d35 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -16,9 +16,8 @@ #include #include +#include #include -#include -#include #include // for operator += using namespace boost::assign; @@ -57,25 +56,24 @@ TEST( Pose3, constructors) } /* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP TEST( Pose3, retract_first_order) { Pose3 id; Vector v = zero(6); v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::FIRST_ORDER),1e-2)); + EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v),1e-2)); v(3)=0.2;v(4)=0.7;v(5)=-2; - EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::FIRST_ORDER),1e-2)); + EXPECT(assert_equal(Pose3(R, P),id.retract(v),1e-2)); } - +#endif /* ************************************************************************* */ TEST( Pose3, retract_expmap) { - Pose3 id; - Vector v = zero(6); - v(0) = 0.3; - EXPECT(assert_equal(Pose3(R, Point3()), id.retract(v, Pose3::EXPMAP),1e-2)); - v(3)=0.2;v(4)=0.394742;v(5)=-2.08998; - EXPECT(assert_equal(Pose3(R, P),id.retract(v, Pose3::EXPMAP),1e-2)); + Vector v = zero(6); v(0) = 0.3; + Pose3 pose = Pose3::Expmap(v); + EXPECT(assert_equal(Pose3(R, Point3()), pose, 1e-2)); + EXPECT(assert_equal(v,Pose3::Logmap(pose),1e-2)); } /* ************************************************************************* */ @@ -151,11 +149,11 @@ Pose3 Agrawal06iros(const Vector& xi) { Vector v = xi.tail(3); double t = norm_2(w); if (t < 1e-5) - return Pose3(Rot3(), Point3::Expmap(v)); + return Pose3(Rot3(), Point3(v)); else { Matrix W = skewSymmetric(w/t); Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); - return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v)); + return Pose3(Rot3::Expmap (w), Point3(A * v)); } } @@ -463,43 +461,51 @@ TEST( Pose3, transformPose_to) } /* ************************************************************************* */ -TEST(Pose3, localCoordinates_first_order) +TEST(Pose3, Retract_LocalCoordinates) +{ + Vector6 d; + d << 1,2,3,4,5,6; d/=10; + R = Rot3::Retract(d.head<3>()); + Pose3 t = Pose3::Retract(d); + EXPECT(assert_equal(d, Pose3::LocalCoordinates(t))); +} +/* ************************************************************************* */ +TEST(Pose3, retract_localCoordinates) +{ + Vector6 d12; + d12 << 1,2,3,4,5,6; d12/=10; + Pose3 t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} +/* ************************************************************************* */ +TEST(Pose3, expmap_logmap) { Vector d12 = repeat(6,0.1); - Pose3 t1 = T, t2 = t1.retract(d12, Pose3::FIRST_ORDER); - EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::FIRST_ORDER))); + Pose3 t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); } /* ************************************************************************* */ -TEST(Pose3, localCoordinates_expmap) -{ - Vector d12 = repeat(6,0.1); - Pose3 t1 = T, t2 = t1.retract(d12, Pose3::EXPMAP); - EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::EXPMAP))); -} - -/* ************************************************************************* */ -TEST(Pose3, manifold_first_order) +TEST(Pose3, retract_localCoordinates2) { Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; - Vector d12 = t1.localCoordinates(t2, Pose3::FIRST_ORDER); - EXPECT(assert_equal(t2, t1.retract(d12, Pose3::FIRST_ORDER))); - Vector d21 = t2.localCoordinates(t1, Pose3::FIRST_ORDER); - EXPECT(assert_equal(t1, t2.retract(d21, Pose3::FIRST_ORDER))); + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); } - /* ************************************************************************* */ TEST(Pose3, manifold_expmap) { Pose3 t1 = T; Pose3 t2 = T3; Pose3 origin; - Vector d12 = t1.localCoordinates(t2, Pose3::EXPMAP); - EXPECT(assert_equal(t2, t1.retract(d12, Pose3::EXPMAP))); - Vector d21 = t2.localCoordinates(t1, Pose3::EXPMAP); - EXPECT(assert_equal(t1, t2.retract(d21, Pose3::EXPMAP))); + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); // Check that log(t1,t2)=-log(t2,t1) EXPECT(assert_equal(d12,-d21)); @@ -672,26 +678,28 @@ TEST(Pose3, align_2) { } /* ************************************************************************* */ -/// exp(xi) exp(y) = exp(xi + dxi) -/// Hence, y = log (exp(-xi)*exp(xi+dxi)) -Vector6 xi = (Vector(6) << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0).finished(); - -Vector6 testExpmapDerivative(const Vector6& xi, const Vector6& dxi) { - return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); -} - -TEST( Pose3, ExpmapDerivative) { - Matrix actualDexpL = Pose3::ExpmapDerivative(xi); - Matrix expectedDexpL = numericalDerivative11( - boost::bind(testExpmapDerivative, xi, _1), zero(6), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - - Matrix actualDexpInvL = Pose3::LogmapDerivative(xi); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); +TEST( Pose3, ExpmapDerivative1) { + Matrix6 actualH; + Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; + Pose3::Expmap(w,actualH); + Matrix expectedH = numericalDerivative21 >(&Pose3::Expmap, w, boost::none); + EXPECT(assert_equal(expectedH, actualH)); } /* ************************************************************************* */ -Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) { +TEST( Pose3, LogmapDerivative1) { + Matrix6 actualH; + Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0; + Pose3 p = Pose3::Expmap(w); + EXPECT(assert_equal(w, Pose3::Logmap(p,actualH), 1e-5)); + Matrix expectedH = numericalDerivative21 >(&Pose3::Logmap, p, boost::none); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { return Pose3::adjointMap(xi) * v; } @@ -701,7 +709,7 @@ TEST( Pose3, adjoint) { Matrix actualH; Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); - Matrix numericalH = numericalDerivative21( + Matrix numericalH = numericalDerivative21( testDerivAdjoint, screw::xi, screw::xi, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); @@ -709,7 +717,7 @@ TEST( Pose3, adjoint) { } /* ************************************************************************* */ -Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { +Vector6 testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { return Pose3::adjointMap(xi).transpose() * v; } @@ -721,7 +729,7 @@ TEST( Pose3, adjointTranspose) { Matrix actualH; Vector actual = Pose3::adjointTranspose(xi, v, actualH); - Matrix numericalH = numericalDerivative21( + Matrix numericalH = numericalDerivative21( testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); @@ -737,6 +745,43 @@ TEST( Pose3, stream) EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n"); } +//****************************************************************************** +TEST(Pose3 , Invariants) { + Pose3 id; + + check_group_invariants(id,id); + check_group_invariants(id,T3); + check_group_invariants(T2,id); + check_group_invariants(T2,T3); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,T3); + check_manifold_invariants(T2,id); + check_manifold_invariants(T2,T3); + +} + +//****************************************************************************** +TEST(Pose3 , LieGroupDerivatives) { + Pose3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T3); + +} + +//****************************************************************************** +TEST(Pose3 , ChartDerivatives) { + Pose3 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T3); +} + /* ************************************************************************* */ int main(){ TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp new file mode 100644 index 000000000..7302754d7 --- /dev/null +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testQuaternion.cpp + * @brief Unit tests for Quaternion, as a GTSAM-adapted Lie Group + * @author Frank Dellaert + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +typedef Quaternion Q; // Typedef +typedef traits::ChartJacobian QuaternionJacobian; + +//****************************************************************************** +TEST(Quaternion , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Quaternion , Constructor) { + Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); +} + +//****************************************************************************** +TEST(Quaternion , Invariants) { + Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + check_group_invariants(q1, q2); + check_manifold_invariants(q1, q2); +} + +//****************************************************************************** +TEST(Quaternion , Local) { + Vector3 z_axis(0, 0, 1); + Q q1(Eigen::AngleAxisd(0, z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + QuaternionJacobian H1, H2; + Vector3 expected(0, 0, 0.1); + Vector3 actual = traits::Local(q1, q2, H1, H2); + EXPECT(assert_equal((Vector)expected,actual)); +} + +//****************************************************************************** +TEST(Quaternion , Retract) { + Vector3 z_axis(0, 0, 1); + Q q(Eigen::AngleAxisd(0, z_axis)); + Q expected(Eigen::AngleAxisd(0.1, z_axis)); + Vector3 v(0, 0, 0.1); + QuaternionJacobian Hq, Hv; + Q actual = traits::Retract(q, v, Hq, Hv); + EXPECT(actual.isApprox(expected)); +} + +//****************************************************************************** +TEST(Quaternion , Compose) { + Vector3 z_axis(0, 0, 1); + Q q1(Eigen::AngleAxisd(0.2, z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + + Q expected = q1 * q2; + Matrix actualH1, actualH2; + Q actual = traits::Compose(q1, q2, actualH1, actualH2); + EXPECT(traits::Equals(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(traits::Compose, q1, q2); + EXPECT(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22(traits::Compose, q1, q2); + EXPECT(assert_equal(numericalH2,actualH2)); +} + +//****************************************************************************** +TEST(Quaternion , Between) { + Vector3 z_axis(0, 0, 1); + Q q1(Eigen::AngleAxisd(0.2, z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + + Q expected = q1.inverse() * q2; + Matrix actualH1, actualH2; + Q actual = traits::Between(q1, q2, actualH1, actualH2); + EXPECT(traits::Equals(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(traits::Between, q1, q2); + EXPECT(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22(traits::Between, q1, q2); + EXPECT(assert_equal(numericalH2,actualH2)); +} + +//****************************************************************************** +TEST(Quaternion , Inverse) { + Vector3 z_axis(0, 0, 1); + Q q1(Eigen::AngleAxisd(0.1, z_axis)); + Q expected(Eigen::AngleAxisd(-0.1, z_axis)); + + Matrix actualH; + Q actual = traits::Inverse(q1, actualH); + EXPECT(traits::Equals(expected,actual)); + + Matrix numericalH = numericalDerivative11(traits::Inverse, q1); + EXPECT(assert_equal(numericalH,actualH)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 45fecb244..37054fd83 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -15,10 +15,10 @@ * @author Frank Dellaert */ -#include -#include -#include #include +#include +#include +#include using namespace gtsam; @@ -155,26 +155,45 @@ TEST( Rot2, relativeBearing ) CHECK(assert_equal(expectedH,actualH)); } -/* ************************************************************************* */ -Vector w = (Vector(1) << 0.27).finished(); +//****************************************************************************** +Rot2 T1(0.1); +Rot2 T2(0.2); + +//****************************************************************************** +TEST(Rot2 , Invariants) { + Rot2 id; + + check_group_invariants(id,id); + check_group_invariants(id,T1); + check_group_invariants(T2,id); + check_group_invariants(T2,T1); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,T1); + check_manifold_invariants(T2,id); + check_manifold_invariants(T2,T1); -// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? -// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector1 testDexpL(const Vector& dw) { - Vector1 y = Rot2::Logmap(Rot2::Expmap(-w) * Rot2::Expmap(w + dw)); - return y; } -TEST( Rot2, ExpmapDerivative) { - Matrix actualDexpL = Rot2::ExpmapDerivative(w); - Matrix expectedDexpL = numericalDerivative11( - boost::function( - boost::bind(testDexpL, _1)), Vector(zero(1)), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); +//****************************************************************************** +TEST(Rot2 , LieGroupDerivatives) { + Rot2 id; - Matrix actualDexpInvL = Rot2::LogmapDerivative(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T1); + +} + +//****************************************************************************** +TEST(Rot2 , ChartDerivatives) { + Rot2 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T1); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index a8874fbfd..96346e382 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -18,11 +18,10 @@ #include #include - +#include #include #include #include -#include #include @@ -38,12 +37,18 @@ static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); static double error = 1e-9, epsilon = 0.001; +//****************************************************************************** +TEST(Rot3 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + /* ************************************************************************* */ TEST( Rot3, chart) { Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1).finished(); Rot3 rot3(R); - CHECK_CHART_CONCEPT(rot3); } /* ************************************************************************* */ @@ -148,10 +153,10 @@ TEST( Rot3, retract) Vector v = zero(3); CHECK(assert_equal(R, R.retract(v))); - // test Canonical coordinates - Canonical chart; - Vector v2 = chart.local(R); - CHECK(assert_equal(R, chart.retract(v2))); +// // test Canonical coordinates +// Canonical chart; +// Vector v2 = chart.local(R); +// CHECK(assert_equal(R, chart.retract(v2))); } /* ************************************************************************* */ @@ -214,6 +219,30 @@ TEST(Rot3, log) CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) } +/* ************************************************************************* */ +TEST(Rot3, retract_localCoordinates) +{ + Vector3 d12 = repeat(3,0.1); + Rot3 R2 = R.retract(d12); + EXPECT(assert_equal(d12, R.localCoordinates(R2))); +} +/* ************************************************************************* */ +TEST(Rot3, expmap_logmap) +{ + Vector3 d12 = repeat(3,0.1); + Rot3 R2 = R.expmap(d12); + EXPECT(assert_equal(d12, R.logmap(R2))); +} + +/* ************************************************************************* */ +TEST(Rot3, retract_localCoordinates2) +{ + Rot3 t1 = R, t2 = R*R, origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); +} /* ************************************************************************* */ Vector w = Vector3(0.1, 0.27, -0.2); @@ -288,12 +317,10 @@ TEST(Rot3, manifold_expmap) Rot3 origin; // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP))); + Vector d12 = Rot3::Logmap(gR1.between(gR2)); + Vector d21 = Rot3::Logmap(gR2.between(gR1)); - // Check that it is expmap + // Check expmap CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); @@ -590,6 +617,12 @@ TEST(Rot3, quaternion) { } /* ************************************************************************* */ +Matrix Cayley(const Matrix& A) { + Matrix::Index n = A.cols(); + const Matrix I = eye(n); + return (I-A)*inverse(I+A); +} + TEST( Rot3, Cayley ) { Matrix A = skewSymmetric(1,2,-3); Matrix Q = Cayley(A); @@ -618,6 +651,47 @@ TEST( Rot3, slerp) EXPECT(assert_equal(R1, R1.slerp(0.5,R1))); } +//****************************************************************************** +Rot3 T1(Rot3::rodriguez(Vector3(0, 0, 1), 1)); +Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2)); + +//****************************************************************************** +TEST(Rot3 , Invariants) { + Rot3 id; + + check_group_invariants(id,id); + check_group_invariants(id,T1); + check_group_invariants(T2,id); + check_group_invariants(T2,T1); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,T1); + check_manifold_invariants(T2,id); + check_manifold_invariants(T2,T1); + +} + +//****************************************************************************** +TEST(Rot3 , LieGroupDerivatives) { + Rot3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T1); + +} + +//****************************************************************************** +TEST(Rot3 , ChartDerivatives) { + Rot3 id; + + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,T2); + CHECK_CHART_DERIVATIVES(T2,id); + CHECK_CHART_DERIVATIVES(T2,T1); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index a25db07f6..12fb94bbc 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -10,21 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file testRot3.cpp - * @brief Unit tests for Rot3 class + * @file testRot3M.cpp + * @brief Unit tests for Rot3 class, matrix version * @author Alireza Fathi * @author Frank Dellaert */ -#include #include - #include -#include -#include - -#include - #include #ifndef GTSAM_USE_QUATERNIONS @@ -46,38 +39,10 @@ TEST(Rot3, manifold_cayley) Rot3 origin; // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::CAYLEY); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CAYLEY))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::CAYLEY); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CAYLEY))); - - // Check that log(t1,t2)=-log(t2,t1) - CHECK(assert_equal(d12,-d21)); - - // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector3(0.1, 0.2, 0.3); - // exp(-d)=inverse(exp(d)) - CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); - // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Rot3 R2 = Rot3::Expmap (2 * d); - Rot3 R3 = Rot3::Expmap (3 * d); - Rot3 R5 = Rot3::Expmap (5 * d); - CHECK(assert_equal(R5,R2*R3)); - CHECK(assert_equal(R5,R3*R2)); -} - -/* ************************************************************************* */ -TEST(Rot3, manifold_slow_cayley) -{ - Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); - Rot3 origin; - - // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::SLOW_CAYLEY); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CAYLEY))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CAYLEY); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CAYLEY))); + Vector d12 = gR1.localCayley(gR2); + CHECK(assert_equal(gR2, gR1.retractCayley(d12))); + Vector d21 = gR2.localCayley(gR1); + CHECK(assert_equal(gR1, gR2.retractCayley(d21))); // Check that log(t1,t2)=-log(t2,t1) CHECK(assert_equal(d12,-d21)); diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp new file mode 100644 index 000000000..3d8e41af5 --- /dev/null +++ b/gtsam/geometry/tests/testSO3.cpp @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testQuaternion.cpp + * @brief Unit tests for SO3, as a GTSAM-adapted Lie Group + * @author Frank Dellaert + **/ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +TEST(SO3 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(SO3 , Constructor) { + SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); +} + +//****************************************************************************** +SO3 id; +Vector3 z_axis(0, 0, 1); +SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); +SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); + +//****************************************************************************** +TEST(SO3 , Local) { + Vector3 expected(0, 0, 0.1); + Vector3 actual = traits::Local(R1, R2); + EXPECT(assert_equal((Vector)expected,actual)); +} + +//****************************************************************************** +TEST(SO3 , Retract) { + Vector3 v(0, 0, 0.1); + SO3 actual = traits::Retract(R1, v); + EXPECT(actual.isApprox(R2)); +} + +//****************************************************************************** +TEST(SO3 , Compose) { + SO3 expected = R1 * R2; + Matrix actualH1, actualH2; + SO3 actual = traits::Compose(R1, R2, actualH1, actualH2); + EXPECT(traits::Equals(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(traits::Compose, R1, R2); + EXPECT(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22(traits::Compose, R1, R2); + EXPECT(assert_equal(numericalH2,actualH2)); +} + +//****************************************************************************** +TEST(SO3 , Between) { + SO3 expected = R1.inverse() * R2; + Matrix actualH1, actualH2; + SO3 actual = traits::Between(R1, R2, actualH1, actualH2); + EXPECT(traits::Equals(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(traits::Between, R1, R2); + EXPECT(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22(traits::Between, R1, R2); + EXPECT(assert_equal(numericalH2,actualH2)); +} + +//****************************************************************************** +TEST(SO3 , Inverse) { + SO3 expected(Eigen::AngleAxisd(-0.1, z_axis)); + + Matrix actualH; + SO3 actual = traits::Inverse(R1, actualH); + EXPECT(traits::Equals(expected,actual)); + + Matrix numericalH = numericalDerivative11(traits::Inverse, R1); + EXPECT(assert_equal(numericalH,actualH)); +} + +//****************************************************************************** +TEST(SO3 , Invariants) { + check_group_invariants(id,id); + check_group_invariants(id,R1); + check_group_invariants(R2,id); + check_group_invariants(R2,R1); + + check_manifold_invariants(id,id); + check_manifold_invariants(id,R1); + check_manifold_invariants(R2,id); + check_manifold_invariants(R2,R1); + +} + +//****************************************************************************** +//TEST(SO3 , LieGroupDerivatives) { +// CHECK_LIE_GROUP_DERIVATIVES(id,id); +// CHECK_LIE_GROUP_DERIVATIVES(id,R2); +// CHECK_LIE_GROUP_DERIVATIVES(R2,id); +// CHECK_LIE_GROUP_DERIVATIVES(R2,R1); +//} + +//****************************************************************************** +TEST(SO3 , ChartDerivatives) { + CHECK_CHART_DERIVATIVES(id,id); + CHECK_CHART_DERIVATIVES(id,R2); + CHECK_CHART_DERIVATIVES(R2,id); + CHECK_CHART_DERIVATIVES(R2,R1); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** + diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 8e504ba0e..5496b8aac 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -26,9 +26,14 @@ using namespace std; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) -GTSAM_CONCEPT_LIE_INST(StereoPoint2) +//GTSAM_CONCEPT_LIE_INST(StereoPoint2) +//****************************************************************************** +TEST(StereoPoint2 , Concept) { + BOOST_CONCEPT_ASSERT((IsManifold)); +} + /* ************************************************************************* */ TEST(StereoPoint2, constructor) { StereoPoint2 p1(1, 2, 3), p2 = p1; @@ -49,7 +54,7 @@ TEST(StereoPoint2, Lie) { } /* ************************************************************************* */ -TEST( StereoPoint2, expmap) { +TEST( StereoPoint2, retract) { Vector d(3); d(0) = 1; d(1) = -1; diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 072f3b7ad..1d0c28ded 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -115,13 +115,13 @@ TEST(Unit3, error) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), q); p.error(q, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&Unit3::error, &p, _1, boost::none), r); p.error(r, actual); EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 043c4caf6..adb1c0aad 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -250,7 +250,6 @@ namespace gtsam { void print(const std::string& s = "FactorGraph", const KeyFormatter& formatter = DefaultKeyFormatter) const; - protected: /** Check equality */ bool equals(const This& fg, double tol = 1e-9) const; /// @} diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 4b125988c..23936afed 100644 --- a/gtsam/inference/LabeledSymbol.h +++ b/gtsam/inference/LabeledSymbol.h @@ -130,5 +130,8 @@ inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); /** Return the index portion of a symbol key. */ inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); } +/// traits +template<> struct traits : public Testable {}; + } // \namespace gtsam diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index 24c811841..e25590578 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -191,5 +191,9 @@ namespace gtsam { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; + + /// traits + template<> struct traits : public Testable {}; + } diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 6963905e0..4f9734639 100644 --- a/gtsam/inference/Symbol.h +++ b/gtsam/inference/Symbol.h @@ -18,9 +18,9 @@ #pragma once -#include - #include +#include +#include namespace gtsam { @@ -161,5 +161,8 @@ inline Key Y(size_t j) { return Symbol('y', j); } inline Key Z(size_t j) { return Symbol('z', j); } } -} // namespace gtsam +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 3985221d3..bcaec6ee4 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -17,16 +17,18 @@ #pragma once +#include +#include +#include +#include +#include +#include + +#include + #include #include #include -#include - -#include -#include -#include -#include -#include namespace gtsam { @@ -175,6 +177,11 @@ protected: /// @} }; -} +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam #include diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index 9a16ca788..fad789436 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -22,12 +22,12 @@ #include #include #include - -#include +#include #include #include +#include #include namespace gtsam { @@ -82,6 +82,9 @@ public: /// @} }; +/// traits +template<> struct traits : public Testable {}; + /* ************************************************************************* */ template VariableSlots::VariableSlots(const FG& factorGraph) diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 91b58c2d4..20e0c5262 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -149,6 +149,8 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju << ", alpha = " << alpha << ", beta = " << beta << ", ||r||^2 = " << currentGamma +// << "\nx =\n" << estimate +// << "\nr =\n" << residual << std::endl; } if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index 13da360a5..44d68be83 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -21,6 +21,7 @@ #include #include +#include #include @@ -70,4 +71,9 @@ namespace gtsam { /** print with optional string */ GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error"); -} // gtsam + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 69a70a5e4..a89e7e21c 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -171,4 +171,9 @@ namespace gtsam { } }; -} /// namespace gtsam + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index 5185154b4..1b2ad47e0 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -128,4 +128,9 @@ namespace gtsam { Matrix marginalCovariance(Key key) const; }; -} + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 9cce29d60..e0a820819 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -141,7 +141,11 @@ namespace gtsam { } }; // GaussianConditional -} // gtsam +/// traits +template<> +struct traits : public Testable {}; + +} // \ namespace gtsam #include diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index d7a793d50..16b1f759a 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -20,8 +20,9 @@ #pragma once -#include #include +#include +#include namespace gtsam { @@ -146,4 +147,9 @@ namespace gtsam { }; // GaussianFactor -} // namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 910b25d1e..8653e3629 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -344,4 +344,9 @@ namespace gtsam { //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r); -} // namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 8af5277e2..ec4710dd7 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -443,6 +443,11 @@ namespace gtsam { } }; -} +/// traits +template<> +struct traits : public Testable {}; + +} // \ namespace gtsam + #include diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d33c5e07c..4b44197c6 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -360,7 +360,12 @@ namespace gtsam { } }; // JacobianFactor -} // gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam #include diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index dc6801e03..7aa210dad 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -18,12 +18,14 @@ #pragma once +#include +#include + #include #include #include #include #include -#include #include namespace gtsam { @@ -885,6 +887,13 @@ namespace gtsam { typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Constrained::shared_ptr SharedConstrained; -} // namespace gtsam + /// traits + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + template<> struct traits : public Testable {}; + +} //\ namespace gtsam diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 290249b68..ce33116ab 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -381,4 +381,9 @@ namespace gtsam { } }; // VectorValues definition + /// traits + template<> + struct traits : public Testable { + }; + } // \namespace gtsam diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 7eba995d3..089747dc6 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -24,13 +24,14 @@ namespace gtsam { //*************************************************************************** Vector AttitudeFactor::attitudeError(const Rot3& nRb, - boost::optional H) const { + OptionalJacobian<2, 3> H) const { if (H) { - Matrix D_nRef_R, D_e_nRef; + Matrix23 D_nRef_R; + Matrix22 D_e_nRef; Unit3 nRef = nRb.rotate(bRef_, D_nRef_R); Vector e = nZ_.error(nRef, D_e_nRef); - H->resize(2, 3); - H->block < 2, 3 > (0, 0) = D_e_nRef * D_nRef_R; + + (*H) = D_e_nRef * D_nRef_R; return e; } else { Unit3 nRef = nRb * bRef_; diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index def36cc67..08b75c00d 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -54,7 +54,7 @@ public: /** vector of errors */ Vector attitudeError(const Rot3& p, - boost::optional H = boost::none) const; + OptionalJacobian<2,3> H = boost::none) const; }; /** @@ -131,6 +131,9 @@ private: } }; +/// traits +template<> struct traits : public Testable {}; + /** * Version of AttitudeFactor for Pose3 * @addtogroup Navigation @@ -212,5 +215,8 @@ private: } }; +/// traits +template<> struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index fd761388a..1c22bab9a 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -196,74 +196,68 @@ bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) cons } //------------------------------------------------------------------------------ -Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, +Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, boost::optional H1, boost::optional H2, boost::optional H3, boost::optional H4, boost::optional H5, boost::optional H6) const { - // if we need the jacobians - if(H1 || H2 || H3 || H4 || H5 || H6){ - Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R) - - // error wrt preintegrated measurements - Vector r_pvR(9); - r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, - gravity_, omegaCoriolis_, use2ndOrderCoriolis_, // - H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR); - - // error wrt bias evolution model (random walk) - Vector6 fbias = bias_j.between(bias_i, Hbias_j, Hbias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] - - if(H1) { - H1->resize(15,6); - H1->block<9,6>(0,0) = H1_pvR; - // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] - H1->block<6,6>(9,0) = Z_6x6; - } - if(H2) { - H2->resize(15,3); - H2->block<9,3>(0,0) = H2_pvR; - // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H2->block<6,3>(9,0) = Matrix::Zero(6,3); - } - if(H3) { - H3->resize(15,6); - H3->block<9,6>(0,0) = H3_pvR; - // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] - H3->block<6,6>(9,0) = Z_6x6; - } - if(H4) { - H4->resize(15,3); - H4->block<9,3>(0,0) = H4_pvR; - // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] - H4->block<6,3>(9,0) = Matrix::Zero(6,3); - } - if(H5) { - H5->resize(15,6); - H5->block<9,6>(0,0) = H5_pvR; - // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] - H5->block<6,6>(9,0) = Hbias_i; - } - if(H6) { - H6->resize(15,6); - H6->block<9,6>(0,0) = Matrix::Zero(9,6); - // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] - H6->block<6,6>(9,0) = Hbias_j; - } - Vector r(15); r << r_pvR, fbias; // vector of size 15 - return r; - } - // else, only compute the error vector: - // error wrt preintegrated measurements - Vector r_pvR(9); - r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, - gravity_, omegaCoriolis_, use2ndOrderCoriolis_, // - boost::none, boost::none, boost::none, boost::none, boost::none); // error wrt bias evolution model (random walk) - Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr] + Matrix6 Hbias_i, Hbias_j; + Vector6 fbias = traits::Between(bias_j, bias_i, + H6 ? &Hbias_j : 0, H5 ? &Hbias_i : 0).vector(); + + Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i; + Matrix93 D_r_vel_i, D_r_vel_j; + + // error wrt preintegrated measurements + Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, + bias_i, gravity_, omegaCoriolis_, use2ndOrderCoriolis_, + H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, + H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); + + // if we need the jacobians + if (H1) { + H1->resize(15, 6); + H1->block < 9, 6 > (0, 0) = D_r_pose_i; + // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] + H1->block < 6, 6 > (9, 0) = Z_6x6; + } + if (H2) { + H2->resize(15, 3); + H2->block < 9, 3 > (0, 0) = D_r_vel_i; + // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] + H2->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); + } + if (H3) { + H3->resize(15, 6); + H3->block < 9, 6 > (0, 0) = D_r_pose_j; + // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] + H3->block < 6, 6 > (9, 0) = Z_6x6; + } + if (H4) { + H4->resize(15, 3); + H4->block < 9, 3 > (0, 0) = D_r_vel_j; + // adding: [dBiasAcc/dVi ; dBiasOmega/dVi] + H4->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3); + } + if (H5) { + H5->resize(15, 6); + H5->block < 9, 6 > (0, 0) = D_r_bias_i; + // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] + H5->block < 6, 6 > (9, 0) = Hbias_i; + } + if (H6) { + H6->resize(15, 6); + H6->block < 9, 6 > (0, 0) = Matrix::Zero(9, 6); + // adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j] + H6->block < 6, 6 > (9, 0) = Hbias_j; + } + // overall error - Vector r(15); r << r_pvR, fbias; // vector of size 15 + Vector r(15); + r << r_pvR, fbias; // vector of size 15 return r; } diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index fd769b188..bfd3ebb52 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -24,15 +24,15 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "GPSFactor on " << keyFormatter(this->key()) << "\n"; + cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; nT_.print(" prior mean: "); - this->noiseModel_->print(" noise model: "); + noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nT_.equals(e->nT_, tol); + return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); } //*************************************************************************** @@ -43,8 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p, H->block < 3, 3 > (0, 0) << zeros(3, 3); H->block < 3, 3 > (0, 3) << p.rotation().matrix(); } - // manifold equivalent of h(x)-z -> log(z,h(x)) - return nT_.localCoordinates(p.translation()); + return (p.translation() -nT_).vector(); } //*************************************************************************** diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 466d93203..ce93bd740 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -18,9 +18,6 @@ #pragma once #include -#include -#include -#include #include /* @@ -56,6 +53,10 @@ namespace imuBias { biasAcc_(biasAcc), biasGyro_(biasGyro) { } + ConstantBias(const Vector6& v): + biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { + } + /** return the accelerometer and gyro biases in a single vector */ Vector6 vector() const { Vector6 v; @@ -134,22 +135,6 @@ namespace imuBias { && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol); } - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return dimension; } - - /// return dimensionality of tangent space - inline size_t dim() const { return dimension; } - - /** Update the bias with a tangent space update */ - inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } - - /** @return the local coordinates of another object */ - inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); } - /// @} /// @name Group /// @{ @@ -157,42 +142,32 @@ namespace imuBias { /** identity for group operation */ static ConstantBias identity() { return ConstantBias(); } - /** invert the object and yield a new one */ - inline ConstantBias inverse(boost::optional H=boost::none) const { - if(H) *H = -gtsam::Matrix::Identity(6,6); - + /** inverse */ + inline ConstantBias operator-() const { return ConstantBias(-biasAcc_, -biasGyro_); } - ConstantBias compose(const ConstantBias& b, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = gtsam::Matrix::Identity(6,6); - if(H2) *H2 = gtsam::Matrix::Identity(6,6); - + /** addition */ + ConstantBias operator+(const ConstantBias& b) const { return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); } - /** between operation */ - ConstantBias between(const ConstantBias& b, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -gtsam::Matrix::Identity(6,6); - if(H2) *H2 = gtsam::Matrix::Identity(6,6); - + /** subtraction */ + ConstantBias operator-(const ConstantBias& b) const { return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_); } /// @} - /// @name Lie Group + + /// @name Deprecated /// @{ - - /** Expmap around identity */ - static inline ConstantBias Expmap(const Vector& v) { return ConstantBias(v.head(3), v.tail(3)); } - - /** Logmap around identity - just returns with default cast back */ - static inline Vector Logmap(const ConstantBias& b) { return b.vector(); } - + ConstantBias inverse() { return -(*this);} + ConstantBias compose(const ConstantBias& q) { return (*this)+q;} + ConstantBias between(const ConstantBias& q) { return q-(*this);} + Vector6 localCoordinates(const ConstantBias& q) { return between(q).vector();} + ConstantBias retract(const Vector6& v) {return compose(ConstantBias(v));} + static Vector6 Logmap(const ConstantBias& p) {return p.vector();} + static ConstantBias Expmap(const Vector6& v) { return ConstantBias(v);} /// @} private: @@ -213,27 +188,13 @@ namespace imuBias { /// @} }; // ConstantBias class - - -} // namespace ImuBias - -// Define GTSAM traits -namespace traits { +} // namespace imuBias template<> -struct is_group : public boost::true_type { +struct traits : public internal::VectorSpace< + imuBias::ConstantBias> { }; -template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public boost::integral_constant { -}; - -} - } // namespace gtsam diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 0d7626b95..25b3518bc 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -134,19 +134,19 @@ TEST(AHRSFactor, Error) { // Create factor AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, false); - Vector errorActual = factor.evaluateError(x1, x2, bias); + Vector3 errorActual = factor.evaluateError(x1, x2, bias); // Expected error - Vector errorExpected(3); + Vector3 errorExpected(3); errorExpected << 0, 0, 0; - EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + EXPECT(assert_equal(Vector(errorExpected), Vector(errorActual), 1e-6)); // Expected Jacobians - Matrix H1e = numericalDerivative11( + Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); - Matrix H3e = numericalDerivative11( + Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, x2, _1), bias); // Check rotation Jacobians diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 694163ede..950c6ce63 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -19,7 +19,6 @@ #include #include #include -#include #include @@ -73,7 +72,7 @@ TEST( MagFactor, Factors ) { // MagFactor MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); - EXPECT( assert_equal(numericalDerivative11 // + EXPECT( assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); // MagFactor1 @@ -95,7 +94,7 @@ TEST( MagFactor, Factors ) { // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); - EXPECT(assert_equal(numericalDerivative11 // + EXPECT(assert_equal((Matrix)numericalDerivative11 // (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h index 5118426b4..7295f3160 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -18,19 +18,60 @@ #pragma once -#include -#include +#include #include +#include + +#include +#include namespace gtsam { +namespace detail { + +// By default, we assume an Identity element +template +struct Origin { T operator()() { return traits::Identity();} }; + +// but dimple manifolds don't have one, so we just use the default constructor +template +struct Origin { T operator()() { return T();} }; + +} // \ detail + +/** + * Canonical is a template that creates canonical coordinates for a given type. + * A simple manifold type (i.e., not a Lie Group) has no concept of identity, + * hence in that case we use the value given by the default constructor T() as + * the origin of a "canonical coordinate" parameterization. + */ +template +struct Canonical { + + GTSAM_CONCEPT_MANIFOLD_TYPE(T) + + typedef traits Traits; + enum { dimension = Traits::dimension }; + typedef typename Traits::TangentVector TangentVector; + typedef typename Traits::structure_category Category; + typedef detail::Origin Origin; + + static TangentVector Local(const T& other) { + return Traits::Local(Origin()(), other); + } + + static T Retract(const TangentVector& v) { + return Traits::Retract(Origin()(), v); + } +}; + /// Adapt ceres-style autodiff template class AdaptAutoDiff { - static const int N = traits::dimension::value; - static const int M1 = traits::dimension::value; - static const int M2 = traits::dimension::value; + static const int N = traits::dimension; + static const int M1 = traits::dimension; + static const int M2 = traits::dimension; typedef Eigen::Matrix RowMajor1; typedef Eigen::Matrix RowMajor2; @@ -39,14 +80,10 @@ class AdaptAutoDiff { typedef Canonical Canonical1; typedef Canonical Canonical2; - typedef typename CanonicalT::vector VectorT; - typedef typename Canonical1::vector Vector1; - typedef typename Canonical2::vector Vector2; + typedef typename CanonicalT::TangentVector VectorT; + typedef typename Canonical1::TangentVector Vector1; + typedef typename Canonical2::TangentVector Vector2; - // Instantiate function and charts - CanonicalT chartT; - Canonical1 chart1; - Canonical2 chart2; F f; public: @@ -57,8 +94,8 @@ public: using ceres::internal::AutoDiff; // Make arguments - Vector1 v1 = chart1.local(a1); - Vector2 v2 = chart2.local(a2); + Vector1 v1 = Canonical1::Local(a1); + Vector2 v2 = Canonical2::Local(a2); bool success; VectorT result; @@ -84,7 +121,7 @@ public: if (!success) throw std::runtime_error( "AdaptAutoDiff: function call resulted in failure"); - return chartT.retract(result); + return CanonicalT::Retract(result); } }; diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 48445bba5..190457673 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -21,9 +21,7 @@ #include #include -#include -#include -#include +#include #include #include @@ -143,7 +141,7 @@ void handleLeafCase(const Eigen::MatrixBase& dTdA, */ template class ExecutionTrace { - static const int Dim = traits::dimension::value; + static const int Dim = traits::dimension; enum { Constant, Leaf, Function } kind; @@ -299,57 +297,11 @@ public: } }; -//----------------------------------------------------------------------------- -/// Leaf Expression -template > -class LeafExpression: public ExpressionNode { - typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? - - /// The key into values - Key key_; - - /// Constructor with a single key - LeafExpression(Key key) : - key_(key) { - } - // todo: do we need a virtual destructor here too? - - friend class Expression ; - -public: - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys; - keys.insert(key_); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - // get dimension from the chart; only works for fixed dimension charts - map[key_] = traits::dimension::value; - } - - /// Return value - virtual const value_type& value(const Values& values) const { - return dynamic_cast(values.at(key_)); - } - - /// Construct an execution trace for reverse AD - virtual const value_type& traceExecution(const Values& values, - ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - trace.setLeaf(key_); - return dynamic_cast(values.at(key_)); - } - -}; //----------------------------------------------------------------------------- /// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value template -class LeafExpression > : public ExpressionNode { +class LeafExpression : public ExpressionNode { typedef T value_type; /// The key into values @@ -374,7 +326,7 @@ public: /// Return dimensions for each argument virtual void dims(std::map& map) const { - map[key_] = traits::dimension::value; + map[key_] = traits::dimension; } /// Return value @@ -450,15 +402,15 @@ public: /// meta-function to generate fixed-size JacobianTA type template struct Jacobian { - typedef Eigen::Matrix::value, - traits::dimension::value> type; + typedef Eigen::Matrix::dimension, + traits::dimension> type; }; /// meta-function to generate JacobianTA optional reference template struct MakeOptionalJacobian { - typedef OptionalJacobian::value, - traits::dimension::value> type; + typedef OptionalJacobian::dimension, + traits::dimension> type; }; /** @@ -624,7 +576,7 @@ struct FunctionalNode { /// Provide convenience access to Record storage and implement /// the virtual function based interface of CallRecord using the CallRecordImplementor struct Record: public internal::CallRecordImplementor::value>, public Base::Record { + traits::dimension>, public Base::Record { using Base::Record::print; using Base::Record::startReverseAD4; using Base::Record::reverseAD4; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index e63fbc686..2e0b6c01e 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -173,7 +173,7 @@ private: assert(H.size()==keys.size()); // Pre-allocate and zero VerticalBlockMatrix - static const int Dim = traits::dimension::value; + static const int Dim = traits::dimension; VerticalBlockMatrix Ab(dims, Dim); Ab.matrix().setZero(); JacobianMap jacobianMap(keys, Ab); @@ -237,7 +237,7 @@ private: template struct apply_compose { typedef T result_type; - static const int Dim = traits::dimension::value; + static const int Dim = traits::dimension; T operator()(const T& x, const T& y, OptionalJacobian H1 = boost::none, OptionalJacobian H2 = boost::none) const { return x.compose(y, H1, H2); diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 5765eca01..9251aac07 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -42,7 +42,8 @@ namespace gtsam { // Extract the current estimate of x1,P1 VectorValues result = marginal->solve(VectorValues()); - T x = linearizationPoints.at(lastKey).retract(result[lastKey]); + const T& current = linearizationPoints.at(lastKey); + T x = traits::Retract(current, result[lastKey]); // Create a Jacobian Factor from the root node of the produced Bayes Net. // This will act as a prior for the next iteration. @@ -70,9 +71,10 @@ namespace gtsam { // Create a Jacobian Prior Factor directly P_initial. // Since x0 is set to the provided mean, the b vector in the prior will be zero // TODO Frank asks: is there a reason why noiseModel is not simply P_initial ? + int n = traits::GetDimension(x_initial); priorFactor_ = JacobianFactor::shared_ptr( - new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(x_initial.dim()), - noiseModel::Unit::Create(P_initial->dim()))); + new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n), + noiseModel::Unit::Create(n))); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 7bbd14980..4adad676e 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -42,6 +42,11 @@ namespace gtsam { template class ExtendedKalmanFilter { + + // Check that VALUE type is a testable Manifold + BOOST_CONCEPT_ASSERT((IsTestable)); + BOOST_CONCEPT_ASSERT((IsManifold)); + public: typedef boost::shared_ptr > shared_ptr; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 36ebd7033..f7c6d0345 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -29,7 +29,7 @@ namespace gtsam { template VALUE ISAM2::calculateEstimate(Key key) const { const Vector& delta = getDelta()[key]; - return theta_.at(key).retract(delta); + return traits::Retract(theta_.at(key), delta); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index c2e728a2b..9adceee6a 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -633,6 +633,9 @@ protected: }; // ISAM2 +/// traits +template<> struct traits : public Testable {}; + /// Optimize the BayesTree, starting from the root. /// @param replaced Needs to contain /// all variables that are contained in the top of the Bayes tree that has been diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 340f3f6bc..676b3fb85 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -155,5 +155,7 @@ private: }; // \class LinearContainerFactor +template<> struct traits : public Testable {}; + } // \namespace gtsam diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 10174181b..6e348fb58 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -23,6 +23,7 @@ #include #include +#include namespace gtsam { @@ -92,8 +93,9 @@ public: */ NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : - Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_( - feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) { + Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), + j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // + compare_(_compare) { } /** @@ -101,9 +103,9 @@ public: */ NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : - Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_( - feasible), allow_error_(true), error_gain_(error_gain), compare_( - _compare) { + Base(noiseModel::Constrained::All(traits::GetDimension(feasible)), + j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // + compare_(_compare) { } /// @} @@ -113,15 +115,15 @@ public: virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; - gtsam::print(feasible_, "Feasible Point:\n"); - std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; + traits::Print(feasible_, "Feasible Point:\n"); + std::cout << "Variable Dimension: " << traits::GetDimension(feasible_) << std::endl; } /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This* e = dynamic_cast(&f); return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) - && fabs(error_gain_ - e->error_gain_) < tol; + && std::abs(error_gain_ - e->error_gain_) < tol; } /// @} @@ -142,11 +144,11 @@ public: /** error function */ Vector evaluateError(const T& xj, boost::optional H = boost::none) const { - size_t nj = feasible_.dim(); + const size_t nj = traits::GetDimension(feasible_); if (allow_error_) { if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare - return xj.localCoordinates(feasible_); + return traits::Local(xj,feasible_); } else if (compare_(feasible_, xj)) { if (H) *H = eye(nj); @@ -195,6 +197,10 @@ private: }; // \class NonlinearEquality +template +struct traits > : Testable > { +}; + /* ************************************************************************* */ /** * Simple unary equality constraint - fixes a value for a variable @@ -231,8 +237,8 @@ public: * */ NonlinearEquality1(const X& value, Key key, double mu = 1000.0) : - Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key), value_( - value) { + Base( noiseModel::Constrained::All(traits::GetDimension(value), + std::abs(mu)), key), value_(value) { } virtual ~NonlinearEquality1() { @@ -248,9 +254,9 @@ public: Vector evaluateError(const X& x1, boost::optional H = boost::none) const { if (H) - (*H) = eye(x1.dim()); + (*H) = eye(traits::GetDimension(x1)); // manifold equivalent of h(x)-z -> log(z,h(x)) - return value_.localCoordinates(x1); + return traits::Local(value_,x1); } /** Print */ @@ -276,6 +282,10 @@ private: }; // \NonlinearEquality1 +template +struct traits > : Testable > { +}; + /* ************************************************************************* */ /** * Simple binary equality constraint - this constraint forces two factors to @@ -302,7 +312,7 @@ public: ///TODO: comment NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) : - Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) { + Base(noiseModel::Constrained::All(traits::dimension, std::abs(mu)), key1, key2) { } virtual ~NonlinearEquality2() { } @@ -316,12 +326,10 @@ public: /** g(x) with optional derivative2 */ Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - const size_t p = X::Dim(); - if (H1) - *H1 = -eye(p); - if (H2) - *H2 = eye(p); - return x1.localCoordinates(x2); + static const size_t p = traits::dimension; + if (H1) *H1 = -eye(p); + if (H2) *H2 = eye(p); + return traits::Local(x1,x2); } private: @@ -337,5 +345,10 @@ private: }; // \NonlinearEquality2 +template +struct traits > : Testable > { +}; + + }// namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index d7713d0d5..5d9f176b3 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -144,6 +144,10 @@ public: }; // \class NonlinearFactor +/// traits +template<> struct traits : public Testable { +}; + /* ************************************************************************* */ /** * A nonlinear sum-of-squares factor with a zero-mean noise model diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 35e532262..28f72d57d 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -161,5 +161,10 @@ namespace gtsam { } }; -} // namespace +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index e983ccb17..2128b6bb7 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -292,37 +292,13 @@ namespace gtsam { // insert a plain value using the default chart template void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(ChartValue >(val))); + insert(j, static_cast(GenericValue(val))); } - // insert with custom chart type - template - void Values::insert(Key j, const ValueType& val) { - insert(j, static_cast(ChartValue(val))); - } - - // overloaded insert with chart initializer - template - void Values::insert(Key j, const ValueType& val, Chart chart) { - insert(j, static_cast(ChartValue(val, chart))); - } - // update with default chart template void Values::update(Key j, const ValueType& val) { - update(j, static_cast(ChartValue >(val))); - } - - // update with custom chart - template - void Values::update(Key j, const ValueType& val) { - update(j, static_cast(ChartValue(val))); - } - - // update with chart initializer, /todo: perhaps there is a way to init chart from old value... - template - void Values::update(Key j, const ValueType& val, Chart chart) { - update(j, static_cast(ChartValue(val, chart))); + update(j, static_cast(GenericValue(val))); } } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index dcbc2a433..05f39ed4e 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -24,8 +24,12 @@ #pragma once +#include +#include +#include +#include + #include -#include #include #include #include @@ -44,10 +48,6 @@ #include #include -#include -#include -#include - namespace gtsam { // Forward declarations / utilities @@ -523,6 +523,12 @@ namespace gtsam { } }; -} + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam + #include diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 7d31616c5..f3858c818 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -26,7 +26,6 @@ #include #include #include -#include #include @@ -34,11 +33,78 @@ using boost::assign::list_of; using boost::assign::map_list_of; +namespace gtsam { + +// Special version of Cal3Bundler so that default constructor = 0,0,0 +struct Cal: public Cal3Bundler { + Cal(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, double v0 = 0) : + Cal3Bundler(f, k1, k2, u0, v0) { + } + Cal retract(const Vector& d) const { + return Cal(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0()); + } + Vector3 localCoordinates(const Cal& T2) const { + return T2.vector() - vector(); + } +}; + +template<> +struct traits : public internal::Manifold {}; + +// With that, camera below behaves like Snavely's 9-dim vector +typedef PinholeCamera Camera; + +} + using namespace std; using namespace gtsam; -// The DefaultChart of Camera below is laid out like Snavely's 9-dim vector -typedef PinholeCamera Camera; +/* ************************************************************************* */ +// charts +TEST(AdaptAutoDiff, Canonical) { + + Canonical chart1; + EXPECT(chart1.Local(Point2(1, 0))==Vector2(1, 0)); + EXPECT(chart1.Retract(Vector2(1, 0))==Point2(1, 0)); + + Vector v2(2); + v2 << 1, 0; + Canonical chart2; + EXPECT(assert_equal(v2, chart2.Local(Vector2(1, 0)))); + EXPECT(chart2.Retract(v2)==Vector2(1, 0)); + + Canonical chart3; + Eigen::Matrix v1; + v1 << 1; + EXPECT(chart3.Local(1)==v1); + EXPECT_DOUBLES_EQUAL(chart3.Retract(v1), 1, 1e-9); + + Canonical chart4; + Point3 point(1, 2, 3); + Vector v3(3); + v3 << 1, 2, 3; + EXPECT(assert_equal(v3, chart4.Local(point))); + EXPECT(assert_equal(chart4.Retract(v3), point)); + + Canonical chart5; + Pose3 pose(Rot3::identity(), point); + Vector v6(6); + v6 << 0, 0, 0, 1, 2, 3; + EXPECT(assert_equal(v6, chart5.Local(pose))); + EXPECT(assert_equal(chart5.Retract(v6), pose)); + + Canonical chart6; + Cal cal0; + Vector z3 = Vector3::Zero(); + EXPECT(assert_equal(z3, chart6.Local(cal0))); + EXPECT(assert_equal(chart6.Retract(z3), cal0)); + + Canonical chart7; + Camera camera(Pose3(), cal0); + Vector z9 = Vector9::Zero(); + EXPECT(assert_equal(z9, chart7.Local(camera))); + EXPECT(assert_equal(chart7.Retract(z9), camera)); +} /* ************************************************************************* */ // Some Ceres Snippets copied for testing @@ -87,7 +153,7 @@ struct Projective { /* ************************************************************************* */ // Test Ceres AutoDiff -TEST(Expression, AutoDiff) { +TEST(AdaptAutoDiff, AutoDiff) { using ceres::internal::AutoDiff; // Instantiate function @@ -131,7 +197,7 @@ Vector2 adapted(const Vector9& P, const Vector3& X) { throw std::runtime_error("Snavely fail"); } -TEST(Expression, AutoDiff2) { +TEST(AdaptAutoDiff, AutoDiff2) { using ceres::internal::AutoDiff; // Instantiate function @@ -164,10 +230,10 @@ TEST(Expression, AutoDiff2) { /* ************************************************************************* */ // Test AutoDiff wrapper Snavely -TEST(Expression, AutoDiff3) { +TEST(AdaptAutoDiff, AutoDiff3) { // Make arguments - Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal3Bundler(1, 0, 0)); + Camera P(Pose3(Rot3(), Point3(0, 5, 0)), Cal(1, 0, 0)); Point3 X(10, 0, -5); // negative Z-axis convention of Snavely! typedef AdaptAutoDiff Adaptor; @@ -188,11 +254,12 @@ TEST(Expression, AutoDiff3) { Point2 actual2 = snavely(P, X, H1, H2); EXPECT(assert_equal(expected,actual2,1e-9)); EXPECT(assert_equal(E1,H1,1e-8)); + EXPECT(assert_equal(E2,H2,1e-8)); } /* ************************************************************************* */ // Test AutoDiff wrapper in an expression -TEST(Expression, Snavely) { +TEST(AdaptAutoDiff, Snavely) { Expression P(1); Expression X(2); typedef AdaptAutoDiff Adaptor; diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index a8f287d1e..2bdc04d5b 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -32,37 +32,26 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Export all classes derived from Value -BOOST_CLASS_EXPORT(gtsam::Cal3_S2); -BOOST_CLASS_EXPORT(gtsam::Cal3Bundler); -BOOST_CLASS_EXPORT(gtsam::Point3); -BOOST_CLASS_EXPORT(gtsam::Pose3); -BOOST_CLASS_EXPORT(gtsam::Rot3); -BOOST_CLASS_EXPORT(gtsam::PinholeCamera); -BOOST_CLASS_EXPORT(gtsam::PinholeCamera); -BOOST_CLASS_EXPORT(gtsam::PinholeCamera); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); +GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler); +GTSAM_VALUE_EXPORT(gtsam::Point3); +GTSAM_VALUE_EXPORT(gtsam::Pose3); +GTSAM_VALUE_EXPORT(gtsam::Rot3); +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); namespace detail { template struct pack { typedef T type; }; } -#define CHART_VALUE_EXPORT(UNIQUE_NAME, TYPE) \ - typedef gtsam::ChartValue > UNIQUE_NAME; \ - BOOST_CLASS_EXPORT( UNIQUE_NAME ); - /* ************************************************************************* */ typedef PinholeCamera PinholeCal3S2; typedef PinholeCamera PinholeCal3DS2; typedef PinholeCamera PinholeCal3Bundler; -CHART_VALUE_EXPORT(gtsamPoint3Chart, gtsam::Point3); -CHART_VALUE_EXPORT(Cal3S2Chart, PinholeCal3S2); -CHART_VALUE_EXPORT(Cal3DS2Chart, PinholeCal3DS2); -CHART_VALUE_EXPORT(Cal3BundlerChart, PinholeCal3Bundler); - - - /* ************************************************************************* */ static Point3 pt3(1.0, 2.0, 3.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); @@ -73,27 +62,21 @@ static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); static Cal3Bundler cal3(1.0, 2.0, 3.0); TEST (Serialization, TemplatedValues) { - std::cout << __LINE__ << std::endl; EXPECT(equalsObj(pt3)); - std::cout << __LINE__ << std::endl; - ChartValue chv1(pt3); - std::cout << __LINE__ << std::endl; + GenericValue chv1(pt3); EXPECT(equalsObj(chv1)); - std::cout << __LINE__ << std::endl; PinholeCal3S2 pc(pose3,cal1); EXPECT(equalsObj(pc)); - std::cout << __LINE__ << std::endl; + Values values; values.insert(1,pt3); - std::cout << __LINE__ << std::endl; + EXPECT(equalsObj(values)); - std::cout << __LINE__ << std::endl; values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); EXPECT(equalsObj(values)); - std::cout << __LINE__ << std::endl; EXPECT(equalsXML(values)); EXPECT(equalsBinary(values)); } diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 1b1d76d8a..faa285cd5 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -55,22 +55,27 @@ int TestValueData::DestructorCount = 0; class TestValue { TestValueData data_; public: + enum {dimension = 0}; void print(const std::string& str = "") const {} bool equals(const TestValue& other, double tol = 1e-9) const { return true; } size_t dim() const { return 0; } - TestValue retract(const Vector&) const { return TestValue(); } - Vector localCoordinates(const TestValue&) const { return Vector(); } + TestValue retract(const Vector&, + OptionalJacobian H1=boost::none, + OptionalJacobian H2=boost::none) const { + return TestValue(); + } + Vector localCoordinates(const TestValue&, + OptionalJacobian H1=boost::none, + OptionalJacobian H2=boost::none) const { + return Vector(); + } }; namespace gtsam { -namespace traits { -template <> -struct is_manifold : public boost::true_type {}; -template <> -struct dimension : public boost::integral_constant {}; -} +template <> struct traits : public internal::Manifold {}; } + /* ************************************************************************* */ TEST( Values, equals1 ) { @@ -170,9 +175,9 @@ TEST(Values, basic_functions) Values values; const Values& values_c = values; values.insert(2, Vector3()); - values.insert(4, Vector(3)); + values.insert(4, Vector3()); values.insert(6, Matrix23()); - values.insert(8, Matrix(2,3)); + values.insert(8, Matrix23()); // find EXPECT_LONGS_EQUAL(4, values.find(4)->key); diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index 9c5df7ea0..319c74cd5 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -31,6 +31,10 @@ namespace gtsam { template class BetweenFactor: public NoiseModelFactor2 { + // Check that VALUE type is a testable Lie group + BOOST_CONCEPT_ASSERT((IsTestable)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + public: typedef VALUE T; @@ -42,10 +46,6 @@ namespace gtsam { VALUE measured_; /** The measurement */ - /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T) - GTSAM_CONCEPT_TESTABLE_TYPE(T) - public: // shorthand for a smart pointer to a factor @@ -74,26 +74,32 @@ namespace gtsam { std::cout << s << "BetweenFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; - traits::print()(measured_, " measured: "); + traits::Print(measured_, " measured: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::equals()(this->measured_, e->measured_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(this->measured_, e->measured_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const T& p1, const T& p2, - boost::optional H1 = boost::none,boost::optional H2 = - boost::none) const { - T hx = p1.between(p2, H1, H2); // h(x) - DefaultChart chart; + Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + T hx = traits::Between(p1, p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) - return chart.local(measured_, hx); +#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR + typename traits::ChartJacobian::Fixed Hlocal; + Vector rval = traits::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); + if (H1) *H1 = Hlocal * (*H1); + if (H1) *H2 = Hlocal * (*H2); + return rval; +#else + return traits::Local(measured_, hx); +#endif } /** return the measured */ @@ -118,6 +124,10 @@ namespace gtsam { } }; // \class BetweenFactor + /// traits + template + struct traits > : public Testable > {}; + /** * Binary between constraint - forces between to a given value * This constraint requires the underlying type to a Lie type @@ -131,7 +141,7 @@ namespace gtsam { /** Syntactic sugar for constrained version */ BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : BetweenFactor(key1, key2, measured, - noiseModel::Constrained::All(DefaultChart::getDimension(measured), fabs(mu))) + noiseModel::Constrained::All(traits::GetDimension(measured), fabs(mu))) {} private: @@ -145,4 +155,8 @@ namespace gtsam { } }; // \class BetweenConstraint + /// traits + template + struct traits > : public Testable > {}; + } /// namespace gtsam diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 9142f9d3d..2f8dd601f 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -35,16 +35,22 @@ namespace gtsam { * @addtogroup SLAM */ template - class GeneralSFMFactor: public NoiseModelFactor2 { + class GeneralSFMFactor: public NoiseModelFactor2 { + + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) + + static const int DimC = FixedDimension::value; + static const int DimL = FixedDimension::value; + protected: + Point2 measured_; ///< the 2D measurement public: - typedef CAMERA Cam; ///< typedef for camera type - typedef GeneralSFMFactor This; ///< typedef for this object + typedef GeneralSFMFactor This; ///< typedef for this object typedef NoiseModelFactor2 Base; ///< typedef for the base class - typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -89,7 +95,7 @@ namespace gtsam { } /** h(x)-z */ - Vector evaluateError(const Cam& camera, const Point3& point, + Vector evaluateError(const CAMERA& camera, const LANDMARK& point, boost::optional H1=boost::none, boost::optional H2=boost::none) const { try { @@ -97,8 +103,8 @@ namespace gtsam { return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, camera.dim()); - if (H2) *H2 = zeros(2, point.dim()); + if (H1) *H1 = zeros(2, DimC); + if (H2) *H2 = zeros(2, DimL); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; return zero(2); @@ -121,21 +127,30 @@ namespace gtsam { } }; + template + struct traits > : Testable< + GeneralSFMFactor > { + }; + /** * Non-linear factor for a constraint derived from a 2D measurement. * Compared to GeneralSFMFactor, it is a ternary-factor because the calibration is isolated from camera.. */ template class GeneralSFMFactor2: public NoiseModelFactor3 { + + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) + static const int DimK = FixedDimension::value; + protected: - Point2 measured_; ///< the 2D measurement + + Point2 measured_; ///< the 2D measurement public: typedef GeneralSFMFactor2 This; typedef PinholeCamera Camera; ///< typedef for camera type typedef NoiseModelFactor3 Base; ///< typedef for the base class - typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -189,9 +204,9 @@ namespace gtsam { return reprojError.vector(); } catch( CheiralityException& e) { - if (H1) *H1 = zeros(2, pose3.dim()); - if (H2) *H2 = zeros(2, point.dim()); - if (H3) *H3 = zeros(2, calib.dim()); + if (H1) *H1 = zeros(2, 6); + if (H2) *H2 = zeros(2, 3); + if (H3) *H3 = zeros(2, DimK); std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; } @@ -214,6 +229,9 @@ namespace gtsam { } }; - + template + struct traits > : Testable< + GeneralSFMFactor2 > { + }; } //namespace diff --git a/gtsam/slam/PoseRotationPrior.h b/gtsam/slam/PoseRotationPrior.h index 7d31e2e2c..f7d42497e 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -29,6 +29,10 @@ public: GTSAM_CONCEPT_GROUP_TYPE(Pose) GTSAM_CONCEPT_LIE_TYPE(Rotation) + // Get dimensions of pose and rotation type at compile time + static const int xDim = FixedDimension::value; + static const int rDim = FixedDimension::value; + protected: Rotation measured_; @@ -70,7 +74,6 @@ public: /** h(x)-z */ Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { const Rotation& newR = pose.rotation(); - const size_t rDim = newR.dim(), xDim = pose.dim(); if (H) { *H = gtsam::zeros(rDim, xDim); std::pair rotInterval = POSE::rotationInterval(); diff --git a/gtsam/slam/PoseTranslationPrior.h b/gtsam/slam/PoseTranslationPrior.h index 2d9b3fdd4..6689653aa 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -62,7 +62,8 @@ public: Vector evaluateError(const Pose& pose, boost::optional H = boost::none) const { const Translation& newTrans = pose.translation(); const Rotation& R = pose.rotation(); - const size_t tDim = newTrans.dim(), xDim = pose.dim(); + const int tDim = traits::GetDimension(newTrans); + const int xDim = traits::GetDimension(pose); if (H) { *H = gtsam::zeros(tDim, xDim); std::pair transInterval = POSE::translationInterval(); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 8fbbd4d88..eee14f9f2 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -67,24 +67,24 @@ namespace gtsam { /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n"; - traits::print()(prior_, " prior mean: "); + traits::Print(prior_, " prior mean: "); this->noiseModel_->print(" noise model: "); } /** equals */ virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This* e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && traits::equals()(prior_, e->prior_, tol); + return e != NULL && Base::equals(*e, tol) && traits::Equals(prior_, e->prior_, tol); } /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const T& p, boost::optional H = boost::none) const { - DefaultChart chart; - if (H) (*H) = eye(chart.getDimension(p)); + if (H) (*H) = eye(traits::GetDimension(p)); // manifold equivalent of h(x)-z -> log(z,h(x)) - return chart.local(prior_,p); + // TODO(ASL) Add Jacobians. + return traits::Local(prior_,p); } const VALUE & prior() const { return prior_; } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index db37e6b8d..52e28beaf 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -187,4 +187,11 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); } }; + + /// traits + template + struct traits > : + public Testable > { + }; + } // \ namespace gtsam diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 88c122f6e..216b9320e 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -96,8 +96,11 @@ private: boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } -}; -// RangeFactor +}; // \ RangeFactor + +/// traits +template +struct traits > : public Testable > {}; /** * Binary factor for a range measurement, with a transform applied @@ -185,7 +188,10 @@ private: ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } -}; -// RangeFactor +}; // \ RangeFactorWithTransform -}// namespace gtsam +/// traits +template +struct traits > : public Testable > {}; + +} // \ namespace gtsam diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 13dec7de3..6a70d58b4 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -84,7 +84,7 @@ public: * each degree of freedom. */ ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2) - : Base(noiseModel::Isotropic::Sigma(POINT().dim(), sigma), + : Base(noiseModel::Isotropic::Sigma(POINT::dimension, sigma), globalKey, transKey, localKey) {} virtual ~ReferenceFrameFactor(){} @@ -99,11 +99,9 @@ public: boost::optional Dtrans = boost::none, boost::optional Dlocal = boost::none) const { Point newlocal = transform_point(trans, global, Dtrans, Dforeign); - if (Dlocal) { - Point dummy; - *Dlocal = -1* gtsam::eye(dummy.dim()); - } - return local.localCoordinates(newlocal); + if (Dlocal) + *Dlocal = -1* gtsam::eye(Point::dimension); + return traits::Local(local,newlocal); } virtual void print(const std::string& s="", @@ -130,4 +128,8 @@ private: } }; +/// traits +template +struct traits > : public Testable > {}; + } // \namespace gtsam diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index d9e7b9819..44d6902fa 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -48,7 +48,7 @@ protected: boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras) - static const int ZDim = traits::dimension::value; ///< Measurement dimension + static const int ZDim = traits::dimension; ///< Measurement dimension /// Definitions for blocks of F typedef Eigen::Matrix Matrix2D; // F diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 75e4699d9..2bfcbfaa0 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -104,7 +104,7 @@ protected: /// shorthand for this class typedef SmartProjectionFactor This; - static const int ZDim = traits::dimension::value; ///< Measurement dimension + static const int ZDim = 2; ///< Measurement dimension public: diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 3b2e2bcbc..0801d141f 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -215,4 +215,8 @@ private: }; // end of class declaration +/// traits +template +struct traits > : public Testable > {}; + } // \ namespace gtsam diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index f54ee47ba..7b21e044f 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -180,4 +180,8 @@ private: } }; +/// traits +template +struct traits > : public Testable > {}; + } // \ namespace gtsam diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 5d73bcab8..1dc897599 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -41,7 +41,7 @@ TEST( AntiFactor, NegativeHessian) Pose3 pose1(Rot3(), Point3(0, 0, 0)); Pose3 pose2(Rot3(), Point3(2, 1, 3)); Pose3 z(Rot3(), Point3(1, 1, 1)); - SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); + SharedNoiseModel sigma(noiseModel::Unit::Create(6)); // Create a configuration corresponding to the ground truth Values values; @@ -92,7 +92,7 @@ TEST( AntiFactor, EquivalentBayesNet) Pose3 pose1(Rot3(), Point3(0, 0, 0)); Pose3 pose2(Rot3(), Point3(2, 1, 3)); Pose3 z(Rot3(), Point3(1, 1, 1)); - SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); + SharedNoiseModel sigma(noiseModel::Unit::Create(6)); NonlinearFactorGraph graph; graph.push_back(PriorFactor(1, pose1, sigma)); diff --git a/gtsam/slam/tests/testBetweenFactor.cpp b/gtsam/slam/tests/testBetweenFactor.cpp index b22763099..adb759dc0 100644 --- a/gtsam/slam/tests/testBetweenFactor.cpp +++ b/gtsam/slam/tests/testBetweenFactor.cpp @@ -31,13 +31,13 @@ TEST(BetweenFactor, Rot3) { Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2)); EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail - Matrix numericalH1 = numericalDerivative21( + Matrix numericalH1 = numericalDerivative21( boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); EXPECT(assert_equal(numericalH1,actualH1, 1E-5)); - Matrix numericalH2 = numericalDerivative22( + Matrix numericalH2 = numericalDerivative22( boost::function(boost::bind( &BetweenFactor::evaluateError, factor, _1, _2, boost::none, boost::none)), R1, R2, 1e-5); diff --git a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp index 4c0edf5c9..31b276642 100644 --- a/gtsam/slam/tests/testEssentialMatrixConstraint.cpp +++ b/gtsam/slam/tests/testEssentialMatrixConstraint.cpp @@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) { CHECK(assert_equal(expected, actual, 1e-8)); // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( + Matrix expectedH1 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, boost::none, boost::none), pose1); - Matrix expectedH2 = numericalDerivative11( + Matrix expectedH2 = numericalDerivative11( boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, boost::none, boost::none), pose2); diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index e8acb0db0..e0e26ecff 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/gtsam/slam/tests/testEssentialMatrixFactor.cpp @@ -96,7 +96,8 @@ TEST (EssentialMatrixFactor, factor) { // Use numerical derivatives to calculate the expected Jacobian Matrix Hexpected; - Hexpected = numericalDerivative11( + typedef Eigen::Matrix Vector1; + Hexpected = numericalDerivative11( boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::none), trueE); @@ -173,8 +174,8 @@ TEST (EssentialMatrixFactor2, factor) { boost::function f = boost::bind( &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -247,8 +248,8 @@ TEST (EssentialMatrixFactor3, factor) { boost::function f = boost::bind( &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); @@ -389,8 +390,8 @@ TEST (EssentialMatrixFactor2, extraTest) { boost::function f = boost::bind( &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(f, trueE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); @@ -458,8 +459,8 @@ TEST (EssentialMatrixFactor3, extraTest) { boost::function f = boost::bind( &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 8adbaa62d..240b19a46 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal - Vector z = Vector2(323.,240.); + Point2 z(323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index 9cc634b37..df56f5260 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -86,7 +86,7 @@ static const SharedNoiseModel sigma1(noiseModel::Unit::Create(2)); TEST( GeneralSFMFactor_Cal3Bundler, equals ) { // Create two identical factors and make sure they're equal - Vector z = Vector2(323.,240.); + Point2 z(323.,240.); const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index 9165ff17a..db04a74eb 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -53,7 +53,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) { Pose3RotationPrior factor(poseKey, rot3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -67,7 +67,7 @@ TEST( testPoseRotationFactor, level3_error ) { #else EXPECT(assert_equal(Vector3(-0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2)); #endif - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); // the derivative is more complex, but is close to the identity for Rot3 around the origin // If not using true expmap will be close, but not exact around the origin // EXPECT(assert_equal(expH1, actH1, tol)); @@ -79,7 +79,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) { Pose2RotationPrior factor(poseKey, rot2A, model1); Matrix actH1; EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -89,7 +89,7 @@ TEST( testPoseRotationFactor, level2_error ) { Pose2RotationPrior factor(poseKey, rot2B, model1); Matrix actH1; EXPECT(assert_equal((Vector(1) << -M_PI_2).finished(), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -99,7 +99,7 @@ TEST( testPoseRotationFactor, level2_error_wrap ) { Pose2RotationPrior factor(poseKey, rot2D, model1); Matrix actH1; EXPECT(assert_equal((Vector(1) << -0.02).finished(), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index f7bd412fe..2f39701f7 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -49,7 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -59,7 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -69,7 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -79,7 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -89,7 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -99,7 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -109,7 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) { Pose2TranslationPrior factor(poseKey, point2A, model2); Matrix actH1; EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -119,7 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; EXPECT(assert_equal(Vector2(-3.0,-4.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 2a2e3b40f..9c99628e6 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -69,13 +69,13 @@ TEST (RotateFactor, test) { Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); f.evaluateError(iRc, actual); EXPECT(assert_equal(expected, actual, 1e-9)); } { - expected = numericalDerivative11( + expected = numericalDerivative11( boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); f.evaluateError(R, actual); EXPECT(assert_equal(expected, actual, 1e-9)); diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 2e59a98a4..5dda02350 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -81,4 +81,9 @@ namespace gtsam { } }; -} // namespace gtsam + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index 526120a30..cac711043 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -70,4 +70,9 @@ namespace gtsam { } }; -} +/// traits +template<> struct traits : public Testable {}; +template<> struct traits : public Testable {}; + +} //\ namespace gtsam + diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 01f7366d6..4b89a4b60 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -17,9 +17,10 @@ #pragma once -#include -#include #include +#include +#include +#include namespace gtsam { @@ -121,4 +122,9 @@ namespace gtsam { } }; -} +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicEliminationTree.h b/gtsam/symbolic/SymbolicEliminationTree.h index 5a44d451d..e20fb67bd 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.h +++ b/gtsam/symbolic/SymbolicEliminationTree.h @@ -59,4 +59,9 @@ namespace gtsam { }; -} +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 0054f9fbc..f30f88935 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -17,13 +17,15 @@ #pragma once -#include +#include +#include +#include + #include #include #include -#include -#include +#include namespace gtsam { @@ -158,4 +160,10 @@ namespace gtsam { GTSAM_EXPORT std::pair, boost::shared_ptr > EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys); -} + /// traits + template<> + struct traits : public Testable { + }; + +} //\ namespace gtsam + diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 37aae2400..9813df331 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -116,4 +116,9 @@ namespace gtsam { } }; -} // namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index 1c2807e7a..66538e802 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -100,7 +100,7 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { assert(false); - return zero(x1.dim()); + return Vector6::Zero(); } private: diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 73fc58fa6..5ed079acb 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -90,7 +90,7 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { assert(false); // no corresponding factor here - return zero(x1.dim()); + return Vector6::Zero(); } private: diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 06aa4ac24..818266d4c 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -5,7 +5,7 @@ #include #include -#include + #include #include @@ -57,21 +57,26 @@ void PoseRTV::print(const string& s) const { } /* ************************************************************************* */ -PoseRTV PoseRTV::Expmap(const Vector9& v) { +PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) { + if (H) CONCEPT_NOT_IMPLEMENTED; Pose3 newPose = Pose3::Expmap(v.head<6>()); - Velocity3 newVel = Velocity3::Expmap(v.tail<3>()); + Velocity3 newVel = Velocity3(v.tail<3>()); return PoseRTV(newPose, newVel); } /* ************************************************************************* */ -Vector9 PoseRTV::Logmap(const PoseRTV& p) { +Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) { + if (H) CONCEPT_NOT_IMPLEMENTED; Vector6 Lx = Pose3::Logmap(p.Rt_); - Vector3 Lv = Velocity3::Logmap(p.v_); + Vector3 Lv = p.v_.vector(); return (Vector9() << Lx, Lv).finished(); } /* ************************************************************************* */ -PoseRTV PoseRTV::retract(const Vector& v) const { +PoseRTV PoseRTV::retract(const Vector& v, + ChartJacobian Horigin, + ChartJacobian Hv) const { + if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED; assert(v.size() == 9); // First order approximation Pose3 newPose = Rt_.retract(sub(v, 0, 6)); @@ -80,7 +85,10 @@ PoseRTV PoseRTV::retract(const Vector& v) const { } /* ************************************************************************* */ -Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { +Vector PoseRTV::localCoordinates(const PoseRTV& p1, + ChartJacobian Horigin, + ChartJacobian Hp) const { + if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED; const Pose3& x0 = pose(), &x1 = p1.pose(); // First order approximation Vector6 poseLogmap = x0.localCoordinates(x1); @@ -90,26 +98,25 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { /* ************************************************************************* */ PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } -PoseRTV PoseRTV::inverse(boost::optional H1) const { +PoseRTV PoseRTV::inverse(ChartJacobian H1) const { if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); - return PoseRTV(Rt_.inverse(), v_.inverse()); + return PoseRTV(Rt_.inverse(), - v_); } /* ************************************************************************* */ PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); } -PoseRTV PoseRTV::compose(const PoseRTV& p, - boost::optional H1, - boost::optional H2) const { +PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1, + ChartJacobian H2) const { if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5); if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5); - return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_)); + return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_); } /* ************************************************************************* */ PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); } PoseRTV PoseRTV::between(const PoseRTV& p, - boost::optional H1, - boost::optional H2) const { + ChartJacobian H1, + ChartJacobian H2) const { if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5); if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5); return inverse().compose(p); @@ -180,7 +187,7 @@ PoseRTV PoseRTV::generalDynamics( Rot3 r2 = rotation().retract(gyro * dt); // Integrate Velocity Equations - Velocity3 v2 = v_.compose(Velocity3(dt * (r2.matrix() * accel + g))); + Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g))); // Integrate Position Equations Point3 t2 = translationIntegration(r2, v2, dt); @@ -197,15 +204,15 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Vector6 imu; // acceleration - Vector accel = v1.localCoordinates(v2) / dt; + Vector3 accel = (v2-v1).vector() / dt; imu.head<3>() = r2.transpose() * (accel - g); // rotation rates // just using euler angles based on matlab code // FIXME: this is silly - we shouldn't use differences in Euler angles Matrix Enb = RRTMnb(r1); - Vector euler1 = r1.xyz(), euler2 = r2.xyz(); - Vector dR = euler2 - euler1; + Vector3 euler1 = r1.xyz(), euler2 = r2.xyz(); + Vector3 dR = euler2 - euler1; // normalize yaw in difference (as per Mitch's code) dR(2) = Rot2::fromAngle(dR(2)).theta(); @@ -227,7 +234,7 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub /* ************************************************************************* */ double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); } double PoseRTV::range(const PoseRTV& other, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const { if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5); if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5); return t().distance(other.t()); @@ -238,9 +245,8 @@ PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) { return global.transformed_from(transform); } -PoseRTV PoseRTV::transformed_from(const Pose3& trans, - boost::optional Dglobal, - boost::optional Dtrans) const { +PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, + OptionalJacobian<9, 6> Dtrans) const { // Note that we rotate the velocity Matrix DVr, DTt; Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt); diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index 43d018752..ea7a2c9ff 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -25,7 +25,11 @@ protected: Pose3 Rt_; Velocity3 v_; + typedef OptionalJacobian<9, 9> ChartJacobian; + public: + enum { dimension = 9 }; + // constructors - with partial versions PoseRTV() {} PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel) @@ -78,38 +82,39 @@ public: * - v(3-5): Point3 * - v(6-8): Translational velocity */ - PoseRTV retract(const Vector& v) const; - Vector localCoordinates(const PoseRTV& p) const; + PoseRTV retract(const Vector& v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const; + Vector localCoordinates(const PoseRTV& p, ChartJacobian Horigin=boost::none,ChartJacobian Hp=boost::none) const; - // Lie + // Lie TODO IS this a Lie group or just a Manifold???? /** * expmap/logmap are poor approximations that assume independence of components * Currently implemented using the poor retract/unretract approximations */ - static PoseRTV Expmap(const Vector9& v); - static Vector9 Logmap(const PoseRTV& p); + static PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none); + static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none); static PoseRTV identity() { return PoseRTV(); } /** Derivatives calculated numerically */ - PoseRTV inverse(boost::optional H1=boost::none) const; + PoseRTV inverse(ChartJacobian H1=boost::none) const; /** Derivatives calculated numerically */ PoseRTV compose(const PoseRTV& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + ChartJacobian H1=boost::none, + ChartJacobian H2=boost::none) const; + PoseRTV operator*(const PoseRTV& p) const { return compose(p); } /** Derivatives calculated numerically */ PoseRTV between(const PoseRTV& p, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + ChartJacobian H1=boost::none, + ChartJacobian H2=boost::none) const; // measurement functions /** Derivatives calculated numerically */ double range(const PoseRTV& other, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1,9> H1=boost::none, + OptionalJacobian<1,9> H2=boost::none) const; // IMU-specific @@ -156,8 +161,8 @@ public: * Note: the transform jacobian convention is flipped */ PoseRTV transformed_from(const Pose3& trans, - boost::optional Dglobal=boost::none, - boost::optional Dtrans=boost::none) const; + ChartJacobian Dglobal = boost::none, + OptionalJacobian<9, 6> Dtrans = boost::none) const; // Utility functions @@ -183,23 +188,8 @@ private: } }; -// Define GTSAM traits -namespace traits { template<> -struct is_manifold : public boost::true_type { -}; +struct traits : public internal::LieGroupTraits {}; -template<> -struct dimension : public boost::integral_constant { -}; - -template<> -struct zero { - static PoseRTV value() { - return PoseRTV(); - } -}; - -} } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 88f0acdce..a3dd6327b 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -7,9 +7,10 @@ #pragma once -#include -#include #include +#include +#include +#include namespace gtsam { @@ -29,7 +30,7 @@ class Reconstruction : public NoiseModelFactor3 { typedef NoiseModelFactor3 Base; public: Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : - Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey, + Base(noiseModel::Constrained::All(6, std::abs(mu)), gKey1, gKey, xiKey), h_(h) { } virtual ~Reconstruction() {} @@ -45,27 +46,26 @@ public: boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { - Matrix D_gkxi_gk, D_gkxi_exphxi; - Pose3 gkxi = gk.compose(Pose3::Expmap(h_*xik), D_gkxi_gk, D_gkxi_exphxi); + Matrix6 D_exphxi_xi; + Pose3 exphxi = Pose3::Expmap(h_ * xik, H3 ? &D_exphxi_xi : 0); - Matrix D_hx_gk1, D_hx_gkxi; - Pose3 hx = gkxi.between(gk1, D_hx_gkxi, D_hx_gk1); + Matrix6 D_gkxi_gk, D_gkxi_exphxi; + Pose3 gkxi = gk.compose(exphxi, D_gkxi_gk, H3 ? &D_gkxi_exphxi : 0); - if (H1) { - *H1 = D_hx_gk1; - } - if (H2) { - Matrix D_hx_gk = D_hx_gkxi * D_gkxi_gk; - *H2 = D_hx_gk; + Matrix6 D_hx_gk1, D_hx_gkxi; + Pose3 hx = gkxi.between(gk1, (H2 || H3) ? &D_hx_gkxi : 0, H1 ? &D_hx_gk1 : 0); + + Matrix6 D_log_hx; + Vector error = Pose3::Logmap(hx, D_log_hx); + + if (H1) *H1 = D_log_hx * D_hx_gk1; + if (H2 || H3) { + Matrix6 D_log_gkxi = D_log_hx * D_hx_gkxi; + if (H2) *H2 = D_log_gkxi * D_gkxi_gk; + if (H3) *H3 = D_log_gkxi * D_gkxi_exphxi * D_exphxi_xi * h_; } - if (H3) { - Matrix D_exphxi_xi = Pose3::ExpmapDerivative(h_*xik)*h_; - Matrix D_hx_xi = D_hx_gkxi * D_gkxi_exphxi * D_exphxi_xi; - *H3 = D_hx_xi; - } - - return Pose3::Logmap(hx); + return error; } }; @@ -92,7 +92,7 @@ public: DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, double h, const Matrix& Inertia, const Vector& Fu, double m, double mu = 1000.0) : - Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), xiKey1, xiKey_1, gKey), + Base(noiseModel::Constrained::All(6, std::abs(mu)), xiKey1, xiKey_1, gKey), h_(h), Inertia_(Inertia), Fu_(Fu), m_(m) { } virtual ~DiscreteEulerPoincareHelicopter() {} diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index 56850a0fb..d97f185e7 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp @@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), (Vector)Vector3(2.0, 2.0, 0.0)), - pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), (Vector)Vector3(0.0, 0.0, 0.0)), - pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), (Vector)Vector3(2.0, 2.0, 0.0)); + pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Point3(2.0, 2.0, 0.0)), + pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Point3(0.0, 0.0, 0.0)), + pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Point3(2.0, 2.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Unit::Create(6); @@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) { // create a simple chain of poses to generate IMU measurements const double dt = 1.0; PoseRTV pose1, - pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)), - pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)), - pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), (Vector)Vector3(1.0, 0.0, 0.0)); + pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)), + pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)), + pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)); // create measurements SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 34bcdea86..a0d969c4d 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -21,7 +21,7 @@ Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); //Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0).finished()); Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0).finished()); Vector6 V1_g1 = g1.inverse().Adjoint(V1_w); -Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); +Pose3 g2(g1.expmap(h*V1_g1)); //Vector6 v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; @@ -45,22 +45,6 @@ Vector computeFu(const Vector& gamma, const Vector& control) { return F*control; } -/* ************************************************************************* */ -Vector testExpmapDeriv(const Vector6& v) { - return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v)); -} - -TEST(Reconstruction, ExpmapInvDeriv) { - Matrix numericalExpmap = numericalDerivative11( - boost::function( - boost::bind(testExpmapDeriv, _1) - ), - Vector6(Vector::Zero(6)), 1e-5 - ); - Matrix dExpInv = Pose3::ExpmapDerivative(h*V1_g1); - EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); -} - /* ************************************************************************* */ TEST( Reconstruction, evaluateError) { // hard constraints don't need a noise model @@ -68,29 +52,23 @@ TEST( Reconstruction, evaluateError) { // verify error function Matrix H1, H2, H3; - EXPECT(assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); - + EXPECT( + assert_equal(zero(6), constraint.evaluateError(g2, g1, V1_g1, H1, H2, H3), tol)); Matrix numericalH1 = numericalDerivative31( boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) - ), - g2, g1, V1_g1, 1e-5 - ); + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, + boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); Matrix numericalH2 = numericalDerivative32( boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) - ), - g2, g1, V1_g1, 1e-5 - ); + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, + boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); Matrix numericalH3 = numericalDerivative33( boost::function( - boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) - ), - g2, g1, V1_g1, 1e-5 - ); + boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, + boost::none, boost::none, boost::none)), g2, g1, V1_g1, 1e-5); EXPECT(assert_equal(numericalH1,H1,1e-5)); EXPECT(assert_equal(numericalH2,H2,1e-5)); diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp index 7e4802393..56d38714a 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp @@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2; const double dt = 1.0; PoseRTV origin, - pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), (Vector)Vector3(1.0, 0.0, 0.0)), + pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)), pose1a(Point3(0.5, 0.0, 0.0)), - pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), (Vector)Vector3(1.0, 0.0, 0.0)); + pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)); /* ************************************************************************* */ TEST( testVelocityConstraint, trapezoidal ) { diff --git a/gtsam_unstable/geometry/BearingS2.h b/gtsam_unstable/geometry/BearingS2.h index 4c84bbe56..3c9247048 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -99,4 +99,7 @@ private: }; +/// traits +template<> struct traits : public Testable {}; + } // \namespace gtsam diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 22aab5d44..c48508fa0 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -140,18 +140,8 @@ private: }; // \class Pose3Upright -// Define GTSAM traits -namespace traits { - template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public boost::integral_constant { -}; - -} +struct traits : public internal::Manifold {}; } // \namespace gtsam diff --git a/gtsam_unstable/geometry/SimWall2D.cpp b/gtsam_unstable/geometry/SimWall2D.cpp index eb8ddace6..9087cac2a 100644 --- a/gtsam_unstable/geometry/SimWall2D.cpp +++ b/gtsam_unstable/geometry/SimWall2D.cpp @@ -156,7 +156,7 @@ std::pair moveWithBounce(const Pose2& cur_pose, double step_size, // Simple check to make sure norm is on side closest robot if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm)) - norm = norm.inverse(); + norm = - norm; // using the reflection const double inside_bias = 0.05; diff --git a/gtsam_unstable/geometry/SimWall2D.h b/gtsam_unstable/geometry/SimWall2D.h index cf2142af8..38bba2ee3 100644 --- a/gtsam_unstable/geometry/SimWall2D.h +++ b/gtsam_unstable/geometry/SimWall2D.h @@ -68,6 +68,9 @@ namespace gtsam { typedef std::vector SimWall2DVector; + /// traits + template<> struct traits : public Testable {}; + /** * Calculates the next pose in a trajectory constrained by walls, with noise on * angular drift and reflection noise diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h index e6fec98b7..bc1b2bc12 100644 --- a/gtsam_unstable/linear/LinearEquality.h +++ b/gtsam_unstable/linear/LinearEquality.h @@ -113,8 +113,11 @@ public: return 0.0; } -}; -// LinearEquality +}; // \ LinearEquality -}// gtsam + +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearEqualityFactorGraph.h b/gtsam_unstable/linear/LinearEqualityFactorGraph.h index fd9191250..9c067ae3d 100644 --- a/gtsam_unstable/linear/LinearEqualityFactorGraph.h +++ b/gtsam_unstable/linear/LinearEqualityFactorGraph.h @@ -28,5 +28,10 @@ public: typedef boost::shared_ptr shared_ptr; }; -} // namespace gtsam +/// traits +template<> struct traits : public Testable< + LinearEqualityFactorGraph> { +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index 24ad8545c..7c62c3d54 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -136,8 +136,10 @@ public: return aTp; } -}; -// LinearInequality +}; // \ LinearInequality -}// gtsam +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h index b72f2cc3c..eca271941 100644 --- a/gtsam_unstable/linear/LinearInequalityFactorGraph.h +++ b/gtsam_unstable/linear/LinearInequalityFactorGraph.h @@ -43,5 +43,10 @@ public: } }; -} // namespace gtsam +/// traits +template<> struct traits : public Testable< + LinearInequalityFactorGraph> { +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h index 9b2903a14..5c66d411f 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -67,7 +67,7 @@ public: template VALUE calculateEstimate(Key key) const { const Vector delta = delta_.at(key); - return theta_.at(key).retract(delta); + return traits::Retract(theta_.at(key), delta); } /** read the current set of optimizer parameters */ diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h index fce28b214..9a458ee5a 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchFilter.h @@ -253,4 +253,9 @@ void ConcurrentBatchFilter::PrintKeys(const Container& keys, const std::string& /// Typedef for Matlab wrapping typedef ConcurrentBatchFilter::Result ConcurrentBatchFilterResult; -}/// namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h index d85109605..f3808686c 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -204,4 +204,9 @@ private: /// Typedef for Matlab wrapping typedef ConcurrentBatchSmoother::Result ConcurrentBatchSmootherResult; -}/// namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h index 4f2e1b0aa..11012674e 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -198,4 +198,9 @@ private: /// Typedef for Matlab wrapping typedef ConcurrentIncrementalFilter::Result ConcurrentIncrementalFilterResult; -}/// namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h index 5fe6effb2..78bab5079 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -168,4 +168,9 @@ private: /// Typedef for Matlab wrapping typedef ConcurrentIncrementalSmoother::Result ConcurrentIncrementalSmootherResult; -}/// namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 56e72a807..e75add618 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -38,7 +38,7 @@ protected: Expression expression_; ///< the expression that is AD enabled FastVector dims_; ///< dimensions of the Jacobian matrices - static const int Dim = traits::dimension::value; + static const int Dim = traits::dimension; public: @@ -65,13 +65,12 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - DefaultChart chart; - if (H) { + if (H) { const T value = expression_.value(x, keys_, dims_, *H); - return chart.local(measurement_, value); + return traits::Local(measurement_, value); } else { const T value = expression_.value(x); - return chart.local(measurement_, value); + return traits::Local(measurement_, value); } } @@ -99,8 +98,7 @@ public: T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! // Evaluate error and set RHS vector b - DefaultChart chart; - Ab(size()).col(0) = -chart.local(measurement_, value); + Ab(size()).col(0) = -traits::Local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS Vector dummy(Dim); diff --git a/gtsam_unstable/nonlinear/LinearizedFactor.h b/gtsam_unstable/nonlinear/LinearizedFactor.h index 7be219187..72c7c66f5 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -156,8 +156,10 @@ private: } }; - - +/// traits +template<> +struct traits : public Testable { +}; /** * A factor that takes a linear, Hessian factor and inserts it into @@ -269,6 +271,9 @@ private: } }; - +/// traits +template<> +struct traits : public Testable { +}; } // \namespace aspn diff --git a/gtsam_unstable/nonlinear/expressionTesting.h b/gtsam_unstable/nonlinear/expressionTesting.h index 4fd47eb76..bcca51d07 100644 --- a/gtsam_unstable/nonlinear/expressionTesting.h +++ b/gtsam_unstable/nonlinear/expressionTesting.h @@ -107,7 +107,7 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_, const gtsam::Expression& expression, const gtsam::Values& values, double nd_step, double tolerance) { // Create factor - size_t size = traits::dimension::value; + size_t size = traits::dimension; ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); testFactorJacobians(result_, name_, f, values, nd_step, tolerance); diff --git a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp index 1cc674901..15a0cc494 100644 --- a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp +++ b/gtsam_unstable/nonlinear/tests/testCallRecord.cpp @@ -19,12 +19,11 @@ */ #include - -#include - #include #include +#include + using namespace std; using namespace gtsam; @@ -70,6 +69,11 @@ struct CallConfig { } }; +/// traits +namespace gtsam { +template<> struct traits : public Testable {}; +} + struct Record: public internal::CallRecordImplementor { Record() : cc(0, 0) {} virtual ~Record() { diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 5758509eb..fff878738 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -148,6 +148,7 @@ TEST(ExpressionFactor, Wide) { values.insert(2, Point3(0, 0, 1)); Point3_ point(2); Vector9 measured; + measured.setZero(); Expression expression(wide,point); SharedNoiseModel model = noiseModel::Unit::Create(9); diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 810673b64..857c07761 100644 --- a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h +++ b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h @@ -16,13 +16,13 @@ **/ #pragma once -#include - -#include -#include #include #include #include +#include +#include + +#include namespace gtsam { @@ -91,8 +91,8 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - Vector v1( VALUE::Logmap(p1) ); - Vector v2( VALUE::Logmap(p2) ); + Vector v1( traits::Logmap(p1) ); + Vector v2( traits::Logmap(p2) ); Vector alpha(tau_.size()); Vector alpha_v1(tau_.size()); @@ -132,4 +132,9 @@ private: }; // \class GaussMarkov1stOrderFactor +/// traits +template struct traits > : + public Testable > { +}; + } /// namespace gtsam diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 805c0a71a..6b111b759 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -391,7 +391,12 @@ private: boost::serialization::base_object(*this)); } -}; // \class GaussMarkov1stOrderFactor +}; // \class InertialNavFactor_GlobalVelocity +/// traits +template +struct traits > : + public Testable > { +}; } /// namespace aspn diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index c5b1fc2a4..4218a5561 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -25,29 +25,29 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, const Matrix& F, const double g_e, bool flat) { // estimate gyro bias by averaging gyroscope readings (10.62) - Vector x_g = U.rowwise().mean(); - Vector meanF = F.rowwise().mean(); + Vector3 x_g = U.rowwise().mean(); + Vector3 meanF = F.rowwise().mean(); // estimate gravity - Vector b_g; + Vector3 b_g; if(g_e == 0) { if (flat) // acceleration measured is along the z-axis. - b_g = (Vector(3) << 0.0, 0.0, norm_2(meanF)).finished(); + b_g = (Vector3(3) << 0.0, 0.0, norm_2(meanF)).finished(); else // acceleration measured is the opposite of gravity (10.13) b_g = -meanF; } else { if (flat) // gravity is downward along the z-axis since we are flat on the ground - b_g = (Vector(3) << 0.0,0.0,g_e).finished(); + b_g = (Vector3(3) << 0.0,0.0,g_e).finished(); else // normalize b_g and attribute remainder to biases b_g = - g_e * meanF/meanF.norm(); } // estimate accelerometer bias - Vector x_a = meanF + b_g; + Vector3 x_a = meanF + b_g; // initialize roll, pitch (eqns. 10.14, 10.15) double g1=b_g(0); @@ -66,42 +66,42 @@ Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, } /* ************************************************************************* */ -Mechanization_bRn2 Mechanization_bRn2::correct(const Vector& dx) const { - Vector rho = sub(dx, 0, 3); +Mechanization_bRn2 Mechanization_bRn2::correct(const Vector3& dx) const { + Vector3 rho = sub(dx, 0, 3); Rot3 delta_nRn = Rot3::rodriguez(rho); Rot3 bRn = bRn_ * delta_nRn; - Vector x_g = x_g_ + sub(dx, 3, 6); - Vector x_a = x_a_ + sub(dx, 6, 9); + Vector3 x_g = x_g_ + sub(dx, 3, 6); + Vector3 x_a = x_a_ + sub(dx, 6, 9); return Mechanization_bRn2(bRn, x_g, x_a); } /* ************************************************************************* */ -Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector& u, +Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u, const double dt) const { // integrate rotation nRb based on gyro measurement u and bias x_g #ifndef MODEL_NAV_FRAME_ROTATION // get body to inertial (measured in b) from gyro, subtract bias - Vector b_omega_ib = u - x_g_; + Vector3 b_omega_ib = u - x_g_; // nav to inertial due to Earth's rotation and our movement on Earth surface // TODO: figure out how to do this if we need it - Vector b_omega_in = zero(3); + Vector3 b_omega_in; b_omega_in.setZero(); // calculate angular velocity on bRn measured in body frame - Vector b_omega_bn = b_omega_in - b_omega_ib; + Vector3 b_omega_bn = b_omega_in - b_omega_ib; #else // Assume a non-rotating nav frame (probably an approximation) // calculate angular velocity on bRn measured in body frame - Vector b_omega_bn = x_g_ - u; + Vector3 b_omega_bn = x_g_ - u; #endif // convert to navigation frame Rot3 nRb = bRn_.inverse(); - Vector n_omega_bn = (nRb*b_omega_bn).vector(); + Vector3 n_omega_bn = (nRb*b_omega_bn).vector(); // integrate bRn using exponential map, assuming constant over dt Rot3 delta_nRn = Rot3::rodriguez(n_omega_bn*dt); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index dda267a59..fa33ce5b9 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -18,8 +18,8 @@ class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 { private: Rot3 bRn_; ///< rotation from nav to body - Vector x_g_; ///< gyroscope bias - Vector x_a_; ///< accelerometer bias + Vector3 x_g_; ///< gyroscope bias + Vector3 x_a_; ///< accelerometer bias public: @@ -30,7 +30,7 @@ public: * @param r3 Z-axis of rotated frame */ Mechanization_bRn2(const Rot3& initial_bRn = Rot3(), - const Vector& initial_x_g = zero(3), const Vector& initial_x_a = zero(3)) : + const Vector3& initial_x_g = zero(3), const Vector3& initial_x_a = zero(3)) : bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) { } @@ -40,14 +40,14 @@ public: } /// gravity in the body frame - Vector b_g(double g_e) const { - Vector n_g = (Vector(3) << 0, 0, g_e).finished(); + Vector3 b_g(double g_e) const { + Vector3 n_g(0, 0, g_e); return (bRn_ * n_g).vector(); } const Rot3& bRn() const {return bRn_; } - const Vector& x_g() const { return x_g_; } - const Vector& x_a() const { return x_a_; } + const Vector3& x_g() const { return x_g_; } + const Vector3& x_a() const { return x_a_; } /** * Initialize the first Mechanization state @@ -68,7 +68,7 @@ public: * @param obj The current state * @param dx The error used to correct and return a new state */ - Mechanization_bRn2 correct(const Vector& dx) const; + Mechanization_bRn2 correct(const Vector3& dx) const; /** * Integrate to get new state @@ -76,14 +76,14 @@ public: * @param u gyro measurement to integrate * @param dt time elapsed since previous state in seconds */ - Mechanization_bRn2 integrate(const Vector& u, const double dt) const; + Mechanization_bRn2 integrate(const Vector3& u, const double dt) const; /// print void print(const std::string& s = "") const { bRn_.print(s + ".R"); - gtsam::print(x_g_, s + ".x_g"); - gtsam::print(x_a_, s + ".x_a"); + std::cout << s + ".x_g" << x_g_ << std::endl; + std::cout << s + ".x_a" << x_a_ << std::endl; } }; diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 24f083907..991aae1fd 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -88,7 +88,7 @@ namespace gtsam { // manifold equivalent of h(x)-z -> log(z,h(x)) return prior_.localCoordinates(p.compose(*body_P_sensor_, H)); } else { - if(H) (*H) = eye(p.dim()); + if(H) (*H) = I_6x6; // manifold equivalent of h(x)-z -> log(z,h(x)) return prior_.localCoordinates(p); } diff --git a/gtsam_unstable/slam/ProjectionFactorPPP.h b/gtsam_unstable/slam/ProjectionFactorPPP.h index b69f852b4..0426c3ba4 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPP.h +++ b/gtsam_unstable/slam/ProjectionFactorPPP.h @@ -178,4 +178,11 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); } }; + + /// traits + template + struct traits > : + public Testable > { + }; + } // \ namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorPPPC.h b/gtsam_unstable/slam/ProjectionFactorPPPC.h index d3bfbbb7d..5906a6664 100644 --- a/gtsam_unstable/slam/ProjectionFactorPPPC.h +++ b/gtsam_unstable/slam/ProjectionFactorPPPC.h @@ -168,4 +168,11 @@ namespace gtsam { ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); } }; + + /// traits + template + struct traits > : + public Testable > { + }; + } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 1c0d1bc37..5d2ba3323 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -107,7 +107,7 @@ protected: /// shorthand for this class typedef SmartStereoProjectionFactor This; - typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + enum {ZDim = 3}; ///< Dimension trait of measurement type public: @@ -482,7 +482,7 @@ public: if (triangulateForLinearize(cameras)) return Base::createJacobianSVDFactor(cameras, point_, lambda); else - return boost::make_shared< JacobianFactorSVD >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate @@ -745,4 +745,10 @@ private: } }; +/// traits +template +struct traits > : + public Testable > { +}; + } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 1f2bd1942..2e3bc866b 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -215,4 +215,10 @@ private: }; // end of class declaration +/// traits +template +struct traits > : + public Testable > { +}; + } // \ namespace gtsam diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index d4c5f8172..aae4e413d 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -49,7 +49,7 @@ public: boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { Point2 d = pose.transform_to(point, H1, H2); - Point2 e = measured_.between(d); + Point2 e = d - measured_; return e.vector(); } }; @@ -99,12 +99,12 @@ public: *H3 = D_e_point_g * D_point_g_base2; if (H4) *H4 = D_e_point_g * D_point_g_point; - return measured_.localCoordinates(d); + return (d - measured_).vector(); } else { Pose2 pose_g = base1.compose(pose); Point2 point_g = base2.transform_from(point); Point2 d = pose_g.transform_to(point_g); - return measured_.localCoordinates(d); + return (d - measured_).vector(); } } }; diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 249e4fc20..2abc49fa1 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -16,13 +16,13 @@ **/ #pragma once -#include - -#include -#include +#include #include #include -#include +#include +#include + +#include namespace gtsam { @@ -224,4 +224,10 @@ namespace gtsam { } }; // \class TransformBtwRobotsUnaryFactor + /// traits + template + struct traits > : + public Testable > { + }; + } /// namespace gtsam diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h index e3de43628..adfb9ae36 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h @@ -16,15 +16,15 @@ **/ #pragma once -#include - -#include -#include +#include #include #include #include #include -#include +#include +#include + +#include namespace gtsam { @@ -422,4 +422,10 @@ namespace gtsam { } }; // \class TransformBtwRobotsUnaryFactorEM + /// traits + template + struct traits > : + public Testable > { + }; + } /// namespace gtsam diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 2f54528b8..31995e769 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -117,21 +117,21 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* Create GUIDs for geometry */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT(gtsam::LieVector); -BOOST_CLASS_EXPORT(gtsam::LieMatrix); -BOOST_CLASS_EXPORT(gtsam::Point2); -BOOST_CLASS_EXPORT(gtsam::StereoPoint2); -BOOST_CLASS_EXPORT(gtsam::Point3); -BOOST_CLASS_EXPORT(gtsam::Rot2); -BOOST_CLASS_EXPORT(gtsam::Rot3); -BOOST_CLASS_EXPORT(gtsam::Pose2); -BOOST_CLASS_EXPORT(gtsam::Pose3); -BOOST_CLASS_EXPORT(gtsam::Cal3_S2); -BOOST_CLASS_EXPORT(gtsam::Cal3DS2); -BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo); -BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); -BOOST_CLASS_EXPORT(gtsam::SimpleCamera); -BOOST_CLASS_EXPORT(gtsam::StereoCamera); +GTSAM_VALUE_EXPORT(gtsam::LieVector); +GTSAM_VALUE_EXPORT(gtsam::LieMatrix); +GTSAM_VALUE_EXPORT(gtsam::Point2); +GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); +GTSAM_VALUE_EXPORT(gtsam::Point3); +GTSAM_VALUE_EXPORT(gtsam::Rot2); +GTSAM_VALUE_EXPORT(gtsam::Rot3); +GTSAM_VALUE_EXPORT(gtsam::Pose2); +GTSAM_VALUE_EXPORT(gtsam::Pose3); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); +GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); +GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); +GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +GTSAM_VALUE_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 6cfcd93e6..efe1df640 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -218,7 +218,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); + Point3 t2 = t1 + Point3(Vel1 * measurement_dt); Pose3 Pose2(R2, t2); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); Vector3 Vel2 = Vel1 + dv; @@ -568,7 +568,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); + Point3 t2 = t1+ Point3(Vel1 * measurement_dt); Pose3 Pose2(R2, t2); Vector dv = measurement_dt diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index 238bece62..b468a2b6e 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -28,6 +28,13 @@ using namespace gtsam; typedef PoseBetweenFactor TestPoseBetweenFactor; +/// traits +namespace gtsam { +template<> +struct traits : public Testable { +}; +} + /* ************************************************************************* */ TEST( PoseBetweenFactor, Constructor) { Key poseKey1(1); diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index 9aa87fb83..ddb5cf7a2 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -28,6 +28,13 @@ using namespace gtsam; typedef PosePriorFactor TestPosePriorFactor; +/// traits +namespace gtsam { +template<> +struct traits : public Testable { +}; +} + /* ************************************************************************* */ TEST( PosePriorFactor, Constructor) { Key poseKey(1); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp index 0e5f6421f..573352e87 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPP.cpp @@ -48,6 +48,13 @@ using symbol_shorthand::T; typedef ProjectionFactorPPP TestProjectionFactor; +/// traits +namespace gtsam { +template<> +struct traits : public Testable { +}; +} + /* ************************************************************************* */ TEST( ProjectionFactorPPP, nonStandard ) { ProjectionFactorPPP f; diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index a8a3ae5e9..ebb52d276 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -44,10 +44,10 @@ TEST( DeltaFactor, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, boost::none), pose); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, boost::none), point); @@ -78,16 +78,16 @@ TEST( DeltaFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, point, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, point, boost::none, boost::none, boost::none, boost::none), pose); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, point, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, _1, boost::none, boost::none, boost::none, boost::none), point); @@ -119,16 +119,16 @@ TEST( OdometryFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, pose2, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, pose2, boost::none, boost::none, boost::none, boost::none), pose1); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, pose2, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, base2, _1, boost::none, boost::none, boost::none, boost::none), pose2); diff --git a/matlab.h b/matlab.h index c4891a615..349cdeea4 100644 --- a/matlab.h +++ b/matlab.h @@ -143,7 +143,7 @@ void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { sigma); Sampler sampler(model, seed); BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); + values.update(key_value.key, key_value.value + Point2(sampler.sample())); } } @@ -164,7 +164,7 @@ void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { sigma); Sampler sampler(model, seed); BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& key_value, values.filter()) { - values.update(key_value.key, key_value.value.retract(sampler.sample())); + values.update(key_value.key, key_value.value + Point3(sampler.sample())); } } diff --git a/tests/simulated2D.h b/tests/simulated2D.h index b67ef0aef..37d2455eb 100644 --- a/tests/simulated2D.h +++ b/tests/simulated2D.h @@ -18,9 +18,9 @@ // \callgraph #pragma once -#include #include #include +#include // \namespace @@ -270,3 +270,16 @@ namespace simulated2D { }; } // namespace simulated2D + +/// traits +namespace gtsam { +template +struct traits > : Testable< + simulated2D::GenericMeasurement > { +}; + +template<> +struct traits : public Testable { +}; +} + diff --git a/tests/simulated2DConstraints.h b/tests/simulated2DConstraints.h index 6ddf9ebdb..f593ab23a 100644 --- a/tests/simulated2DConstraints.h +++ b/tests/simulated2DConstraints.h @@ -90,11 +90,11 @@ namespace simulated2D { virtual double value(const Point& x, boost::optional H = boost::none) const { if (H) { - Matrix D = zeros(1, x.dim()); + Matrix D = zeros(1, traits::GetDimension(x)); D(0, IDX) = 1.0; *H = D; } - return Point::Logmap(x)(IDX); + return traits::Logmap(x)(IDX); } }; diff --git a/tests/smallExample.h b/tests/smallExample.h index b7cb7aefe..9866d22aa 100644 --- a/tests/smallExample.h +++ b/tests/smallExample.h @@ -349,7 +349,7 @@ inline boost::shared_ptr sharedReallyNonlinearFactor using symbol_shorthand::X; using symbol_shorthand::L; boost::shared_ptr fg(new NonlinearFactorGraph); - Vector z = Vector2(1.0, 0.0); + Point2 z(1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), X(1))); diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 8748e7464..a7bcf7153 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -92,10 +92,7 @@ TEST( ExtendedKalmanFilter, linear ) { // Create Motion Model Factor class NonlinearMotionModel : public NoiseModelFactor2 { -public: - typedef Point2 T; -private: typedef NoiseModelFactor2 Base; typedef NonlinearMotionModel This; @@ -126,19 +123,19 @@ public: virtual ~NonlinearMotionModel() {} // Calculate the next state prediction using the nonlinear function f() - T f(const T& x_t0) const { + Point2 f(const Point2& x_t0) const { // Calculate f(x) double x = x_t0.x() * x_t0.x(); double y = x_t0.x() * x_t0.y(); - T x_t1(x,y); + Point2 x_t1(x,y); // In this example, the noiseModel remains constant return x_t1; } // Calculate the Jacobian of the nonlinear function f() - Matrix F(const T& x_t0) const { + Matrix F(const Point2& x_t0) const { // Calculate Jacobian of f() Matrix F = Matrix(2,2); F(0,0) = 2*x_t0.x(); @@ -150,7 +147,7 @@ public: } // Calculate the inverse square root of the motion model covariance, Q - Matrix QInvSqrt(const T& x_t0) const { + Matrix QInvSqrt(const Point2& x_t0) const { // This motion model has a constant covariance return Q_invsqrt_; } @@ -184,7 +181,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return QInvSqrt(c.at(key1()))*unwhitenedError(c); + return QInvSqrt(c.at(key1()))*unwhitenedError(c); } /** @@ -213,35 +210,29 @@ public: } /** vector of errors */ - Vector evaluateError(const T& p1, const T& p2, + Vector evaluateError(const Point2& p1, const Point2& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { // error = p2 - f(p1) // H1 = d error / d p1 = -d f/ d p1 = -F // H2 = d error / d p2 = I - T prediction = f(p1); + Point2 prediction = f(p1); - if(H1){ + if(H1) *H1 = -F(p1); - } - if(H2){ + if(H2) *H2 = eye(dim()); - } // Return the error between the prediction and the supplied value of p2 - return prediction.localCoordinates(p2); + return (p2 - prediction).vector(); } }; // Create Measurement Model Factor class NonlinearMeasurementModel : public NoiseModelFactor1 { -public: - typedef Point2 T; - -private: typedef NoiseModelFactor1 Base; typedef NonlinearMeasurementModel This; @@ -268,7 +259,7 @@ public: // Calculate the predicted measurement using the nonlinear function h() // Byproduct: updates Jacobian H, and noiseModel (R) - Vector h(const T& x_t1) const { + Vector h(const Point2& x_t1) const { // Calculate prediction Vector z_hat(1); @@ -277,7 +268,7 @@ public: return z_hat; } - Matrix H(const T& x_t1) const { + Matrix H(const Point2& x_t1) const { // Update Jacobian Matrix H(1,2); H(0,0) = 4*x_t1.x() - x_t1.y() - 2.5; @@ -287,7 +278,7 @@ public: } // Calculate the inverse square root of the motion model covariance, Q - Matrix RInvSqrt(const T& x_t0) const { + Matrix RInvSqrt(const Point2& x_t0) const { // This motion model has a constant covariance return R_invsqrt_; } @@ -320,7 +311,7 @@ public: /** Vector of errors, whitened ! */ Vector whitenedError(const Values& c) const { - return RInvSqrt(c.at(key()))*unwhitenedError(c); + return RInvSqrt(c.at(key()))*unwhitenedError(c); } /** @@ -345,7 +336,7 @@ public: } /** vector of errors */ - Vector evaluateError(const T& p, boost::optional H1 = boost::none) const { + Vector evaluateError(const Point2& p, boost::optional H1 = boost::none) const { // error = z - h(p) // H = d error / d p = -d h/ d p = -H Vector z_hat = h(p); diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp index 1927ba5c6..f6180fb73 100644 --- a/tests/testManifold.cpp +++ b/tests/testManifold.cpp @@ -22,7 +22,8 @@ #include #include #include -#include +#include +#include #include #undef CHECK @@ -38,173 +39,120 @@ using namespace gtsam; // The DefaultChart of Camera below is laid out like Snavely's 9-dim vector typedef PinholeCamera Camera; -/* ************************************************************************* */ -// is_manifold -TEST(Manifold, _is_manifold) { - using namespace traits; - EXPECT(!is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); - EXPECT(is_manifold::value); +//****************************************************************************** +TEST(Manifold, SomeManifoldsGTSAM) { + //BOOST_CONCEPT_ASSERT((IsManifold)); // integer is not a manifold + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); } -/* ************************************************************************* */ +//****************************************************************************** +TEST(Manifold, SomeLieGroupsGTSAM) { + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); +} + +//****************************************************************************** +TEST(Manifold, SomeVectorSpacesGTSAM) { + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** // dimension TEST(Manifold, _dimension) { - using namespace traits; - EXPECT_LONGS_EQUAL(2, dimension::value); - EXPECT_LONGS_EQUAL(8, dimension::value); - EXPECT_LONGS_EQUAL(1, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); - EXPECT_LONGS_EQUAL(Eigen::Dynamic, dimension::value); + //using namespace traits; + EXPECT_LONGS_EQUAL(2, traits::dimension); + EXPECT_LONGS_EQUAL(8, traits::dimension); + EXPECT_LONGS_EQUAL(1, traits::dimension); } -/* ************************************************************************* */ +//****************************************************************************** +TEST(Manifold, Identity) { + EXPECT_DOUBLES_EQUAL(0.0, traits::Identity(), 0.0); + EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::Identity()))); + EXPECT(assert_equal(Pose3(), traits::Identity())); + EXPECT(assert_equal(Point2(), traits::Identity())); +} + +//****************************************************************************** // charts TEST(Manifold, DefaultChart) { - DefaultChart chart1; - EXPECT(chart1.local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); - EXPECT(chart1.retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); + //DefaultChart chart1; + EXPECT(traits::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0)); + EXPECT(traits::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0)); Vector v2(2); v2 << 1, 0; - DefaultChart chart2; - EXPECT(assert_equal(v2, chart2.local(Vector2(0, 0), Vector2(1, 0)))); - EXPECT(chart2.retract(Vector2(0, 0), v2) == Vector2(1, 0)); + //DefaultChart chart2; + EXPECT(assert_equal(v2, traits::Local(Vector2(0, 0), Vector2(1, 0)))); + EXPECT(traits::Retract(Vector2(0, 0), v2) == Vector2(1, 0)); { typedef Matrix2 ManifoldPoint; ManifoldPoint m; - DefaultChart chart; + //DefaultChart chart; m << 1, 3, 2, 4; // m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)! - EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(chart.local(ManifoldPoint::Zero(), m)))); - EXPECT(chart.retract(m, Vector4(1, 2, 3, 4)) == 2 * m); + EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m); } { typedef Eigen::Matrix ManifoldPoint; ManifoldPoint m; - DefaultChart chart; + //DefaultChart chart; m << 1, 2; - EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(chart.local(ManifoldPoint::Zero(), m)))); - EXPECT(chart.retract(m, Vector2(1, 2)) == 2 * m); + EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits::Retract(m, Vector2(1, 2)) == 2 * m); } { typedef Eigen::Matrix ManifoldPoint; ManifoldPoint m; - DefaultChart chart; + //DefaultChart chart; m << 1; - EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(chart.local(ManifoldPoint::Zero(), m)))); - EXPECT(chart.retract(m, ManifoldPoint::Ones()) == 2 * m); + EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits::Local(ManifoldPoint::Zero(), m)))); + EXPECT(traits::Retract(m, ManifoldPoint::Ones()) == 2 * m); } - DefaultChart chart3; + //DefaultChart chart3; Vector v1(1); v1 << 1; - EXPECT(assert_equal(v1, chart3.local(0, 1))); - EXPECT_DOUBLES_EQUAL(chart3.retract(0, v1), 1, 1e-9); + EXPECT(assert_equal(v1, traits::Local(0, 1))); + EXPECT_DOUBLES_EQUAL(traits::Retract(0, v1), 1, 1e-9); // Dynamic does not work yet ! Vector z = zero(2), v(2); v << 1, 0; - DefaultChart chart4; - EXPECT(assert_equal(chart4.local(z, v), v)); - EXPECT(assert_equal(chart4.retract(z, v), v)); + //DefaultChart chart4; +// EXPECT(assert_equal(traits::Local(z, v), v)); +// EXPECT(assert_equal(traits::Retract(z, v), v)); Vector v3(3); v3 << 1, 1, 1; Rot3 I = Rot3::identity(); Rot3 R = I.retract(v3); - DefaultChart chart5; - EXPECT(assert_equal(v3, chart5.local(I, R))); - EXPECT(assert_equal(chart5.retract(I, v3), R)); + //DefaultChart chart5; + EXPECT(assert_equal(v3, traits::Local(I, R))); + EXPECT(assert_equal(traits::Retract(I, v3), R)); // Check zero vector - DefaultChart chart6; - EXPECT(assert_equal(zero(3), chart6.local(R, R))); + //DefaultChart chart6; + EXPECT(assert_equal(zero(3), traits::Local(R, R))); } -/* ************************************************************************* */ -// zero -TEST(Manifold, _zero) { - EXPECT(assert_equal(Pose3(), traits::zero::value())); - Cal3Bundler cal(0, 0, 0); - EXPECT(assert_equal(cal, traits::zero::value())); - EXPECT(assert_equal(Camera(Pose3(), cal), traits::zero::value())); - EXPECT(assert_equal(Point2(), traits::zero::value())); - EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::zero::value()))); - EXPECT_DOUBLES_EQUAL(0.0, traits::zero::value(), 0.0); -} - -/* ************************************************************************* */ -// identity -TEST(Manifold, _identity) { - EXPECT(assert_equal(Pose3(), traits::identity::value())); - EXPECT(assert_equal(Cal3Bundler(), traits::identity::value())); - EXPECT(assert_equal(Camera(), traits::identity::value())); - EXPECT(assert_equal(Point2(), traits::identity::value())); - EXPECT(assert_equal(Matrix(Matrix24::Zero()), Matrix(traits::identity::value()))); - EXPECT_DOUBLES_EQUAL(0.0, traits::identity::value(), 0.0); -} - -/* ************************************************************************* */ -// charts -TEST(Manifold, Canonical) { - - Canonical chart1; - EXPECT(chart1.local(Point2(1, 0))==Vector2(1, 0)); - EXPECT(chart1.retract(Vector2(1, 0))==Point2(1, 0)); - - Vector v2(2); - v2 << 1, 0; - Canonical chart2; - EXPECT(assert_equal(v2, chart2.local(Vector2(1, 0)))); - EXPECT(chart2.retract(v2)==Vector2(1, 0)); - - Canonical chart3; - Eigen::Matrix v1; - v1 << 1; - EXPECT(chart3.local(1)==v1); - EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9); - - Canonical chart4; - Point3 point(1, 2, 3); - Vector v3(3); - v3 << 1, 2, 3; - EXPECT(assert_equal(v3, chart4.local(point))); - EXPECT(assert_equal(chart4.retract(v3), point)); - - Canonical chart5; - Pose3 pose(Rot3::identity(), point); - Vector v6(6); - v6 << 0, 0, 0, 1, 2, 3; - EXPECT(assert_equal(v6, chart5.local(pose))); - EXPECT(assert_equal(chart5.retract(v6), pose)); - - Canonical chart6; - Cal3Bundler cal0(0, 0, 0); - Camera camera(Pose3(), cal0); - Vector z9 = Vector9::Zero(); - EXPECT(assert_equal(z9, chart6.local(camera))); - EXPECT(assert_equal(chart6.retract(z9), camera)); - - Cal3Bundler cal; // Note !! Cal3Bundler() != zero::value() - Camera camera2(pose, cal); - Vector v9(9); - v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0; - EXPECT(assert_equal(v9, chart6.local(camera2))); - EXPECT(assert_equal(chart6.retract(v9), camera2)); -} - -/* ************************************************************************* */ +//****************************************************************************** int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//****************************************************************************** diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 96039a3cc..a3c99ece7 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -408,8 +408,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { // odometry constraint eq2D::OdoEqualityConstraint::shared_ptr constraint2( - new eq2D::OdoEqualityConstraint(truth_pt1.between(truth_pt2), key1, - key2)); + new eq2D::OdoEqualityConstraint(truth_pt2-truth_pt1, key1, key2)); NonlinearFactorGraph graph; graph.push_back(constraint1); diff --git a/tests/testPreconditioner.cpp b/tests/testPreconditioner.cpp index 43b4935fc..72bc4e8e3 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -87,18 +87,19 @@ TEST(PCGSolver, simpleLinearSystem) { simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); simpleGFG += JacobianFactor(1, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 1, 0, 0, 1).finished(), (Vector(2) << 0, 0).finished(), unit2); + simpleGFG.print("system"); // Expected solution VectorValues expectedSolution; expectedSolution.insert(0, (Vector(2) << 0.100498, -0.196756).finished()); expectedSolution.insert(2, (Vector(2) << -0.0990413, -0.0980577).finished()); expectedSolution.insert(1, (Vector(2) << -0.0973252, 0.100582).finished()); - //expectedSolution.print("Expected"); + expectedSolution.print("Expected"); // Solve the system using direct method VectorValues deltaDirect = simpleGFG.optimize(); EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); - //deltaDirect.print("Direct"); + deltaDirect.print("Direct"); // Solve the system using Preconditioned Conjugate Gradient solver // Common PCG parameters @@ -106,19 +107,19 @@ TEST(PCGSolver, simpleLinearSystem) { pcg->setMaxIterations(500); pcg->setEpsilon_abs(0.0); pcg->setEpsilon_rel(0.0); - //pcg->setVerbosity("ERROR"); + pcg->setVerbosity("ERROR"); // With Dummy preconditioner pcg->preconditioner_ = boost::make_shared(); VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); EXPECT(assert_equal(expectedSolution, deltaPCGDummy, 1e-5)); - //deltaPCGDummy.print("PCG Dummy"); + deltaPCGDummy.print("PCG Dummy"); // With Block-Jacobi preconditioner pcg->preconditioner_ = boost::make_shared(); VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); - //deltaPCGJacobi.print("PCG Jacobi"); + deltaPCGJacobi.print("PCG Jacobi"); } diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index ffde900e9..2514c80d9 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -148,21 +148,21 @@ BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); /* Create GUIDs for geometry */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT(gtsam::LieVector); -BOOST_CLASS_EXPORT(gtsam::LieMatrix); -BOOST_CLASS_EXPORT(gtsam::Point2); -BOOST_CLASS_EXPORT(gtsam::StereoPoint2); -BOOST_CLASS_EXPORT(gtsam::Point3); -BOOST_CLASS_EXPORT(gtsam::Rot2); -BOOST_CLASS_EXPORT(gtsam::Rot3); -BOOST_CLASS_EXPORT(gtsam::Pose2); -BOOST_CLASS_EXPORT(gtsam::Pose3); -BOOST_CLASS_EXPORT(gtsam::Cal3_S2); -BOOST_CLASS_EXPORT(gtsam::Cal3DS2); -BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo); -BOOST_CLASS_EXPORT(gtsam::CalibratedCamera); -BOOST_CLASS_EXPORT(gtsam::SimpleCamera); -BOOST_CLASS_EXPORT(gtsam::StereoCamera); +GTSAM_VALUE_EXPORT(gtsam::LieVector); +GTSAM_VALUE_EXPORT(gtsam::LieMatrix); +GTSAM_VALUE_EXPORT(gtsam::Point2); +GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); +GTSAM_VALUE_EXPORT(gtsam::Point3); +GTSAM_VALUE_EXPORT(gtsam::Rot2); +GTSAM_VALUE_EXPORT(gtsam::Rot3); +GTSAM_VALUE_EXPORT(gtsam::Pose2); +GTSAM_VALUE_EXPORT(gtsam::Pose3); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); +GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); +GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); +GTSAM_VALUE_EXPORT(gtsam::SimpleCamera); +GTSAM_VALUE_EXPORT(gtsam::StereoCamera); /* Create GUIDs for factors */ /* ************************************************************************* */ diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 6f2909b7a..6377649e2 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -39,20 +39,20 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -BOOST_CLASS_EXPORT(Value); -BOOST_CLASS_EXPORT(Pose); -BOOST_CLASS_EXPORT(NonlinearFactor); -BOOST_CLASS_EXPORT(NoiseModelFactor); -BOOST_CLASS_EXPORT(NM1); -BOOST_CLASS_EXPORT(NM2); -BOOST_CLASS_EXPORT(BetweenFactor); -BOOST_CLASS_EXPORT(PriorFactor); -BOOST_CLASS_EXPORT(BR); -BOOST_CLASS_EXPORT(noiseModel::Base); -BOOST_CLASS_EXPORT(noiseModel::Isotropic); -BOOST_CLASS_EXPORT(noiseModel::Gaussian); -BOOST_CLASS_EXPORT(noiseModel::Diagonal); -BOOST_CLASS_EXPORT(noiseModel::Unit); +GTSAM_VALUE_EXPORT(Value); +GTSAM_VALUE_EXPORT(Pose); +GTSAM_VALUE_EXPORT(NonlinearFactor); +GTSAM_VALUE_EXPORT(NoiseModelFactor); +GTSAM_VALUE_EXPORT(NM1); +GTSAM_VALUE_EXPORT(NM2); +GTSAM_VALUE_EXPORT(BetweenFactor); +GTSAM_VALUE_EXPORT(PriorFactor); +GTSAM_VALUE_EXPORT(BR); +GTSAM_VALUE_EXPORT(noiseModel::Base); +GTSAM_VALUE_EXPORT(noiseModel::Isotropic); +GTSAM_VALUE_EXPORT(noiseModel::Gaussian); +GTSAM_VALUE_EXPORT(noiseModel::Diagonal); +GTSAM_VALUE_EXPORT(noiseModel::Unit); double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { // Compute degrees of freedom (observations - variables) diff --git a/timing/timePose2.cpp b/timing/timePose2.cpp index 06ab633c6..92aece7e5 100644 --- a/timing/timePose2.cpp +++ b/timing/timePose2.cpp @@ -32,7 +32,7 @@ using namespace gtsam; /* ************************************************************************* */ Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) { - return between_default(r1, r2); + return r1.inverse() * r2; } /* ************************************************************************* */ diff --git a/timing/timeRot2.cpp b/timing/timeRot2.cpp index 6860914ef..4ad9d7d47 100644 --- a/timing/timeRot2.cpp +++ b/timing/timeRot2.cpp @@ -32,7 +32,7 @@ using namespace gtsam; /* ************************************************************************* */ Rot2 Rot2betweenDefault(const Rot2& r1, const Rot2& r2) { - return between_default(r1, r2); + return r1.inverse() * r2; } /* ************************************************************************* */