diff --git a/.cproject b/.cproject index 70c0e4b04..7b099eb53 100644 --- a/.cproject +++ b/.cproject @@ -600,7 +600,6 @@ make - tests/testBayesTree.run true false @@ -608,7 +607,6 @@ make - testBinaryBayesNet.run true false @@ -656,7 +654,6 @@ make - testSymbolicBayesNet.run true false @@ -664,7 +661,6 @@ make - tests/testSymbolicFactor.run true false @@ -672,7 +668,6 @@ make - testSymbolicFactorGraph.run true false @@ -688,7 +683,6 @@ make - tests/testBayesTree true false @@ -982,38 +976,6 @@ true true - - make - -j5 - timeCameraExpression.run - true - true - true - - - make - -j5 - timeOneCameraExpression.run - true - true - true - - - make - -j5 - timeSFMExpressions.run - true - true - true - - - make - -j5 - timeAdaptAutoDiff.run - true - true - true - make -j5 @@ -1152,7 +1114,6 @@ make - testErrors.run true false @@ -1382,46 +1343,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1504,6 +1425,7 @@ make + testSimulated2DOriented.run true false @@ -1543,6 +1465,7 @@ make + testSimulated2D.run true false @@ -1550,6 +1473,7 @@ make + testSimulated3D.run true false @@ -1563,6 +1487,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1820,7 +1784,6 @@ cpack - -G DEB true false @@ -1828,7 +1791,6 @@ cpack - -G RPM true false @@ -1836,7 +1798,6 @@ cpack - -G TGZ true false @@ -1844,7 +1805,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2018,6 +1978,14 @@ true true + + make + -j2 VERBOSE=1 + check.navigation + true + false + true + make -j2 @@ -2201,6 +2169,14 @@ true true + + make + -j4 + testSO3.run + true + true + true + make -j2 @@ -2699,7 +2675,6 @@ make - testGraph.run true false @@ -2707,7 +2682,6 @@ make - testJunctionTree.run true false @@ -2715,7 +2689,6 @@ make - testSymbolicBayesNetB.run true false @@ -2793,14 +2766,6 @@ true true - - make - -j5 - testExpressionFactor.run - true - true - true - make -j5 @@ -2809,30 +2774,6 @@ true true - - make - -j5 - testAdaptAutoDiff.run - true - true - true - - - make - -j5 - testCallRecord.run - true - true - true - - - make - -j4 - testBasisDecompositions.run - true - true - true - make -j4 @@ -3209,6 +3150,30 @@ true true + + make + -j4 + testAdaptAutoDiff.run + true + true + true + + + make + -j4 + testCallRecord.run + true + true + true + + + make + -j4 + testExpressionFactor.run + true + true + true + make -j4 @@ -3257,6 +3222,46 @@ true true + + make + -j4 + timeAdaptAutoDiff.run + true + true + true + + + make + -j4 + timeCameraExpression.run + true + true + true + + + make + -j4 + timeOneCameraExpression.run + true + true + true + + + make + -j4 + timeSFMExpressions.run + true + true + true + + + make + -j4 + timeIncremental.run + true + true + true + make -j2 @@ -3267,6 +3272,7 @@ make + tests/testGaussianISAM2 true false diff --git a/.gitignore b/.gitignore index f3f1efd5b..d46bddd10 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,4 @@ /build* -/doc* *.pyc *.DS_Store /examples/Data/dubrovnik-3-7-pre-rewritten.txt diff --git a/.settings/org.eclipse.cdt.core.prefs b/.settings/org.eclipse.cdt.core.prefs new file mode 100644 index 000000000..b700d0a81 --- /dev/null +++ b/.settings/org.eclipse.cdt.core.prefs @@ -0,0 +1,6 @@ +eclipse.preferences.version=1 +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/delimiter=\: +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/operation=append +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/value=$PATH\:/opt/local/bin +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true +environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index 3ea6a6318..ff1f1b692 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -80,6 +80,12 @@ protected: testGroup##testName##Instance; \ void testGroup##testName##Test::run (TestResult& result_) +/** + * Use this to disable unwanted tests without commenting them out. + */ +#define TEST_DISABLED(testGroup, testName)\ + void testGroup##testName##Test(TestResult& result_, const std::string& name_) + /* * Convention for tests: * - "EXPECT" is a test that will not end execution of the series of tests diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md new file mode 100644 index 000000000..3c10451be --- /dev/null +++ b/GTSAM-Concepts.md @@ -0,0 +1,206 @@ +GTSAM Concepts +============== + +As discussed in [Generic Programming Techniques](http://www.boost.org/community/generic_programming.html), concepts define + +* associated types +* valid expressions, like functions and values +* invariants +* complexity guarantees + +Below we discuss the most important concepts use in GTSAM, and after that we discuss how they are implemented/used/enforced. + + +Manifold +-------- + +To optimize over continuous types, we assume they are manifolds. This is central to GTSAM and hence discussed in some more detail below. + +[Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space. + +In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented. +In detail, we ask the following are defined in the traits object: + +* values: + * `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimenionality is only defined at runtime, by specifying the value -1. +* types: + * `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix`. + * `ChartJacobian`, a typedef for `OptionalJacobian`. + * `ManifoldType`, a pointer back to the type. + * `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following: + * `gtsam::traits::manifold_tag` -- Everything in this list is expected + * `gtsam::traits::group_tag` -- The functions defined under **Groups** below. + * `gtsam::traits::lie_group_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below. + * `gtsam::traits::vector_space_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below. +* valid expressions: + * `size_t dim = traits::getDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time. + * `v = traits::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p*, where *p* and *q* are elements of the manifold and the result, *v* is an element of the vector space. + * `p = traits::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v*, where *p* is an element of the manifold and the result, *v* is an element of the vector space. +* invariants + * `Retract(p, Local(p,q)) == q` + * `Local(p, Retract(p, v)) == v` + +Group +----- +A [group]("http://en.wikipedia.org/wiki/Group_(mathematics)"") should be well known from grade school :-), and provides a type with a composition operation that is closed, associative, has an identity element, and an inverse for each element. The following should be added to the traits class for a group: + +* valid expressions: + * `r = traits::Compose(p,q)`, where *p*, *q*, and *r* are elements of the manifold. + * `q = traits::Inverse(p)`, where *p* and*q* are elements of the manifold. + * `r = traits::Between(p,q)`, where *p*, *q*, and *r* are elements of the manifold. +* static members: + * `traits::Identity`, a static const member that represents the group's identity element. +* invariants: + * `Compose(p,Inverse(p)) == Identity` + * `Compose(p,Between(p,q)) == q` + * `Between(p,q) == Compose(Inverse(p),q)` +The `gtsam::group::traits` namespace defines the following: +* values: + * `traits::Identity` -- The identity element for this group stored as a static const. + * `traits::group_flavor` -- the flavor of this group's `compose()` operator, either: + * `gtsam::traits::group_multiplicative_tag` for multiplicative operator syntax ,or + * `gtsam::traits::group_additive_tag` for additive operator syntax. + +We do *not* at this time support more than one composition operator per type. Although mathematically possible, it is hardly ever needed, and the machinery to support it would be burdensome and counter-intuitive. + +Also, a type should provide either multiplication or addition operators depending on the flavor of the operation. To distinguish between the two, we will use a tag (see below). + +Lie Group +--------- + +A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should implements both MANIFOLD and GROUP concepts. +However, we now also need to be able to evaluate the derivatives of compose and inverse. +Hence, we have the following extra valid static functions defined in the struct `gtsam::traits`: + +* `r = traits::Compose(p,q,Hq,Hp)` +* `q = traits::Inverse(p,Hp)` +* `r = traits::Between(p,q,Hq,H2p)` + +where above the *H* arguments stand for optional Jacobian arguments. +That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor). + +In addition, a Lie group has a Lie algebra, which affords two extra valid expressions: + +* `v = traits::Logmap(p,Hp)`, the log map, with optional Jacobian +* `p = traits::Expmap(v,Hv)`, the exponential map, with optional Jacobian + +Note that in the Lie group case, the usual valid expressions for Retract and Local can be generated automatically, e.g. + +``` + T Retract(p,v,Hp,Hv) { + T q = Expmap(v,Hqv); + T r = Compose(p,q,Hrp,Hrq); + Hv = Hrq * Hqv; // chain rule + return r; + } +``` + +For Lie groups, the `exponential map` above is the most obvious mapping: it +associates straight lines in the tangent space with geodesics on the manifold +(and it's inverse, the log map). However, there are two cases in which we deviate from this: + +However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`) + +Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method. + +Vector Space +------------ + +While vector spaces are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. I.e.where + + * `Identity == 0` + * `Inverse(p) == -p` + * `Compose(p,q) == p+q` + * `Between(p,q) == q-p` + * `Local(q) == p-q` + * `Retract(v) == p+v` + +This considerably simplifies certain operations. A `VectorSpace` superclass is available to implement the traits. Types that are vector space models include `Matrix`, `Vector`, any fixed or dynamic Eigen Matrix, `Point2`, and `Point3`. + +Testable +-------- +Unit tests heavily depend on the following two functions being defined for all types that need to be tested: + +* valid expressions: + * `Print(p,s)` where s is an optional string + * `Equals(p,q,tol)` where tol is an optional (double) tolerance + +Implementation +============== + +GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the +TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts. + +`gtsam::traits` is our way to associate these concepts with types, +and we also define a limited number of `gtsam::tags` to select the correct implementation +of certain functions at compile time (tag dispatching). + +Traits +------ + +However, a base class is not a good way to implement/check the other concepts, as we would like these +to apply equally well to types that are outside GTSAM control, e.g., `Eigen::VectorXd`. This is where +[traits](http://www.boost.org/doc/libs/1_57_0/libs/type_traits/doc/html/boost_typetraits/background.html) come in. + +We use Eigen-style or STL-style traits, that define *many* properties at once. + +Note that not everything that makes a concept is defined by traits. Valid expressions such as traits::Compose are +defined simply as static functions within the traits class. +Finally, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types, +rather than having to use traits internally. + +Concept Checks +-------------- + +Boost provides a nice way to check whether a given type satisfies a concept. For example, the following + + BOOST_CONCEPT_ASSERT(IsVectorSpace) + +asserts that Point2 indeed is a model for the VectorSpace concept. + +Future Concepts +=============== + + +Group Action +------------ + +Group actions are concepts in and of themselves that can be concept checked (see below). +In particular, a group can *act* on another space. +For example, the [cyclic group of order 6](http://en.wikipedia.org/wiki/Cyclic_group) can rotate 2D vectors around the origin: + + q = R(i)*p + where R(i) = R(60)^i, where R(60) rotates by 60 degrees + +Hence, we formalize by the following extension of the concept: + +* valid expressions: + * `q = traits::Act(g,p)`, for some instance, *p*, of a space *S*, that can be acted upon by the group element *g* to produce *q* in *S*. + * `q = traits::Act(g,p,Hp)`, if the space acted upon is a continuous differentiable manifold. * + +In the latter case, if *S* is an n-dimensional manifold, *Hp* is an output argument that should be +filled with the *nxn* Jacobian matrix of the action with respect to a change in *p*. It typically depends +on the group element *g*, but in most common example will *not* depend on the value of *p*. For example, in +the cyclic group example above, we simply have + + Hp = R(i) + +Note there is no derivative of the action with respect to a change in g. That will only be defined +for Lie groups, which we introduce now. + + +Lie Group Action +---------------- + +When a Lie group acts on a space, we have two derivatives to care about: + + * `gtasm::manifold::traits::act(g,p,Hg,Hp)`, if the space acted upon is a continuous differentiable manifold. + +An example is a *similarity transform* in 3D, which can act on 3D space, like + + q = s*R*p + t + +Note that again the derivative in *p*, *Hp* is simply *s R*, which depends on *g* but not on *p*. +The derivative in *g*, *Hg*, is in general more complex. + +For now, we won't care about Lie groups acting on non-manifolds. diff --git a/README.md b/README.md index 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/doc/Mathematica/dexpInvL_SE2.nb b/doc/Mathematica/dexpInvL_SE2.nb new file mode 100644 index 000000000..d15932768 --- /dev/null +++ b/doc/Mathematica/dexpInvL_SE2.nb @@ -0,0 +1,607 @@ +(* Content-type: application/vnd.wolfram.mathematica *) + +(*** Wolfram Notebook File ***) +(* http://www.wolfram.com/nb *) + +(* CreatedBy='Mathematica 8.0' *) + +(*CacheID: 234*) +(* Internal cache information: +NotebookFileLineBreakTest +NotebookFileLineBreakTest +NotebookDataPosition[ 157, 7] +NotebookDataLength[ 18933, 598] +NotebookOptionsPosition[ 18110, 565] +NotebookOutlinePosition[ 18464, 581] +CellTagsIndexPosition[ 18421, 578] +WindowFrame->Normal*) + +(* Beginning of Notebook Content *) +Notebook[{ +Cell[TextData[{ + "The \[OpenCurlyQuote]right trivialised\[CloseCurlyQuote] tangent of the \ +exponential map, ", + Cell[BoxData[ + FormBox["dexpR", TraditionalForm]], + FormatType->"TraditionalForm"], + ", according to Iserles05an, formula 2.42, pg. 32 can be written as\n\t", + Cell[BoxData[ + FormBox[GridBox[{ + {"\t"}, + { + RowBox[{ + RowBox[{ + RowBox[{"g", "'"}], + SuperscriptBox["g", + RowBox[{"-", "1"}]]}], "=", + RowBox[{ + SubscriptBox["dexpR", "\[Omega]"], "(", + RowBox[{"\[Omega]", "'"}], ")"}]}]} + }], TraditionalForm]], + FormatType->"TraditionalForm"], + "\nwhere ", + Cell[BoxData[ + FormBox[ + RowBox[{"g", "=", + RowBox[{"exp", "(", "\[Omega]", ")"}]}], TraditionalForm]], + FormatType->"TraditionalForm"], + ", and ", + Cell[BoxData[ + FormBox[ + RowBox[{ + RowBox[{"g", "'"}], "=", + RowBox[{ + RowBox[{"exp", "'"}], + RowBox[{"(", "\[Omega]", ")"}]}]}], TraditionalForm]], + FormatType->"TraditionalForm"], + ".\nCompare this to the left Jacobian matrix ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "l"], TraditionalForm]], + FormatType->"TraditionalForm"], + " in Chirikjian11book2, pg. 26, we see that ", + Cell[BoxData[ + FormBox["dexpR", TraditionalForm]], + FormatType->"TraditionalForm"], + " and ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "l"], TraditionalForm]], + FormatType->"TraditionalForm"], + " are the same.\n\nHence, be careful, Iserles\[CloseCurlyQuote]s \ +\[OpenCurlyQuote]", + StyleBox["right", + FontWeight->"Bold"], + " trivialised\[CloseCurlyQuote] tangent of the exponential map ", + Cell[BoxData[ + FormBox["dexpR", TraditionalForm]], + FormatType->"TraditionalForm"], + " is in fact Chirikjian\[CloseCurlyQuote]s ", + StyleBox["left", + FontWeight->"Bold"], + " Jacobian matrix ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "l"], TraditionalForm]], + FormatType->"TraditionalForm"], + "!!!\n\nWe want to compute the s \[OpenCurlyQuote]", + StyleBox["left", + FontWeight->"Bold"], + " trivialised\[CloseCurlyQuote] tangent of the exponential map, ", + Cell[BoxData[ + FormBox["dexpL", TraditionalForm]], + FormatType->"TraditionalForm"], + ", for SE2, hence, we need to use Chirikjian\[CloseCurlyQuote]s ", + StyleBox["right", + FontWeight->"Bold"], + " Jacobian matrix ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "r"], TraditionalForm]], + FormatType->"TraditionalForm"], + " formula in Chirikjian11book2, pg. 36." +}], "Text", + CellChangeTimes->{{3.6279967389044943`*^9, 3.6279968865058002`*^9}, { + 3.6279969695759087`*^9, 3.6279974871811028`*^9}, 3.62799757389325*^9}], + +Cell[BoxData[{ + RowBox[{"Clear", "[", "J", "]"}], "\[IndentingNewLine]", + RowBox[{ + RowBox[{"J", "[", "\[Alpha]_", "]"}], ":=", + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{ + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",", + RowBox[{ + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], "/", "\[Alpha]"}], + ",", " ", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"\[Alpha]", " ", + SubscriptBox["v", "1"]}], "-", + SubscriptBox["v", "2"], "+", + RowBox[{ + SubscriptBox["v", "2"], + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-", + RowBox[{ + SubscriptBox["v", "1"], + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/", + SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + RowBox[{ + RowBox[{"-", + RowBox[{"(", + RowBox[{"1", "-", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}], "/", "\[Alpha]"}], + ",", + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",", " ", + RowBox[{ + RowBox[{"(", + RowBox[{ + SubscriptBox["v", "1"], "+", + RowBox[{"\[Alpha]", " ", + SubscriptBox["v", "2"]}], "-", + RowBox[{ + SubscriptBox["v", "1"], + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-", + RowBox[{ + SubscriptBox["v", "2"], + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/", + SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}]}], "Input", + CellChangeTimes->{{3.627993817228732*^9, 3.6279939547434673`*^9}, { + 3.627993986274671*^9, 3.6279940386007967`*^9}, {3.627995391081044*^9, + 3.627995412846286*^9}, 3.6279954452391644`*^9, {3.627995531089571*^9, + 3.6279955341932592`*^9}, {3.627996429604282*^9, 3.62799643077184*^9}}], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"Jinv", "[", "\[Alpha]_", "]"}], "=", + RowBox[{"Inverse", "[", + RowBox[{"J", "[", "\[Alpha]", "]"}], "]"}]}], + "\[IndentingNewLine]"}]], "Input", + CellChangeTimes->{ + 3.627995475343099*^9, {3.627995548533964*^9, 3.627995559455151*^9}, { + 3.627996438504909*^9, 3.6279964431657553`*^9}}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{ + FractionBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], + RowBox[{"\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",", + FractionBox[ + RowBox[{ + RowBox[{"-", + FractionBox["1", "\[Alpha]"]}], "+", + FractionBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}], + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}]], ",", + FractionBox[ + RowBox[{ + FractionBox[ + SubscriptBox["v", "1"], + SuperscriptBox["\[Alpha]", "3"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "3"]], "+", + FractionBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "3"]], "-", + FractionBox[ + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "3"]], "+", + FractionBox[ + SubscriptBox["v", "2"], + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "2"]]}], + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + FractionBox[ + RowBox[{ + FractionBox["1", "\[Alpha]"], "-", + FractionBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}], + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}]], ",", + FractionBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], + RowBox[{"\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",", + FractionBox[ + RowBox[{ + RowBox[{"-", + FractionBox[ + SubscriptBox["v", "1"], + SuperscriptBox["\[Alpha]", "2"]]}], "+", + FractionBox[ + RowBox[{ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "1"]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SubscriptBox["v", "2"], + SuperscriptBox["\[Alpha]", "3"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "3"]], "+", + FractionBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "3"]], "-", + FractionBox[ + RowBox[{ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + RowBox[{ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ", + SubscriptBox["v", "2"]}], + SuperscriptBox["\[Alpha]", "3"]]}], + RowBox[{ + FractionBox["1", + SuperscriptBox["\[Alpha]", "2"]], "-", + FractionBox[ + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]], "+", + FractionBox[ + SuperscriptBox[ + RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], + SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{ + 3.627995560030972*^9, {3.627996412919798*^9, 3.627996444306521*^9}}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"Jinv", "[", "\[Alpha]", "]"}], "//", "Simplify"}], "//", + "MatrixForm"}]], "Input", + CellChangeTimes->{{3.627993835637863*^9, 3.627993839233502*^9}, { + 3.627994046108457*^9, 3.627994058781851*^9}, {3.627995546842499*^9, + 3.6279955664940767`*^9}}], + +Cell[BoxData[ + TagBox[ + RowBox[{"(", "\[NoBreak]", GridBox[{ + { + RowBox[{ + FractionBox["1", "2"], " ", "\[Alpha]", " ", + RowBox[{"Cot", "[", + FractionBox["\[Alpha]", "2"], "]"}]}], + RowBox[{"-", + FractionBox["\[Alpha]", "2"]}], + FractionBox[ + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"-", "2"}], "+", + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+", + RowBox[{"\[Alpha]", " ", + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ", + SubscriptBox["v", "1"]}], "+", + RowBox[{"\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], " ", + SubscriptBox["v", "2"]}]}], + RowBox[{"2", " ", "\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]}, + { + FractionBox["\[Alpha]", "2"], + RowBox[{ + FractionBox["1", "2"], " ", "\[Alpha]", " ", + RowBox[{"Cot", "[", + FractionBox["\[Alpha]", "2"], "]"}]}], + FractionBox[ + RowBox[{ + RowBox[{ + RowBox[{"(", + RowBox[{"\[Alpha]", "-", + RowBox[{"\[Alpha]", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}]}], ")"}], " ", + SubscriptBox["v", "1"]}], "+", + RowBox[{ + RowBox[{"(", + RowBox[{ + RowBox[{"-", "2"}], "+", + RowBox[{"2", " ", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+", + RowBox[{"\[Alpha]", " ", + RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ", + SubscriptBox["v", "2"]}]}], + RowBox[{"2", " ", "\[Alpha]", " ", + RowBox[{"(", + RowBox[{ + RowBox[{"-", "1"}], "+", + RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]}, + {"0", "0", "1"} + }, + GridBoxAlignment->{ + "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, + "RowsIndexed" -> {}}, + GridBoxSpacings->{"Columns" -> { + Offset[0.27999999999999997`], { + Offset[0.7]}, + Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.2], { + Offset[0.4]}, + Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], + Function[BoxForm`e$, + MatrixForm[BoxForm`e$]]]], "Output", + CellChangeTimes->{ + 3.627993840513033*^9, {3.62799404156531*^9, 3.6279940592345743`*^9}, + 3.627995567356995*^9, 3.627996415136314*^9, 3.6279964490074778`*^9}] +}, Open ]], + +Cell[TextData[{ + "In case ", + Cell[BoxData[ + FormBox[ + RowBox[{"\[Alpha]", "=", "0"}], TraditionalForm]], + FormatType->"TraditionalForm"], + ", we compute the limits of ", + Cell[BoxData[ + FormBox[ + SubscriptBox["J", "r"], TraditionalForm]], + FormatType->"TraditionalForm"], + " and ", + Cell[BoxData[ + FormBox[ + SuperscriptBox[ + SubscriptBox["J", "r"], + RowBox[{"-", "1"}]], TraditionalForm]], + FormatType->"TraditionalForm"], + " as follows" +}], "Text", + CellChangeTimes->{{3.627997495449997*^9, 3.627997524522543*^9}}], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"Limit", "[", + RowBox[{ + RowBox[{"Jinv", "[", "\[Alpha]", "]"}], ",", + RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//", + "MatrixForm"}]], "Input", + CellChangeTimes->{{3.627995572179821*^9, 3.627995606373824*^9}}], + +Cell[BoxData[ + TagBox[ + RowBox[{"(", "\[NoBreak]", GridBox[{ + {"1", "0", + FractionBox[ + SubscriptBox["v", "2"], "2"]}, + {"0", "1", + RowBox[{"-", + FractionBox[ + SubscriptBox["v", "1"], "2"]}]}, + {"0", "0", "1"} + }, + GridBoxAlignment->{ + "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, + "RowsIndexed" -> {}}, + GridBoxSpacings->{"Columns" -> { + Offset[0.27999999999999997`], { + Offset[0.7]}, + Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.2], { + Offset[0.4]}, + Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], + Function[BoxForm`e$, + MatrixForm[BoxForm`e$]]]], "Output", + CellChangeTimes->{{3.627995585954463*^9, 3.627995606858135*^9}, + 3.6279964178087473`*^9, 3.6279964634008904`*^9}] +}, Open ]], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{ + RowBox[{"Limit", "[", + RowBox[{ + RowBox[{"J", "[", "\[Alpha]", "]"}], ",", + RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//", + "MatrixForm"}]], "Input", + CellChangeTimes->{{3.6279956527343893`*^9, 3.627995660697241*^9}}], + +Cell[BoxData[ + TagBox[ + RowBox[{"(", "\[NoBreak]", GridBox[{ + {"1", "0", + RowBox[{"-", + FractionBox[ + SubscriptBox["v", "2"], "2"]}]}, + {"0", "1", + FractionBox[ + SubscriptBox["v", "1"], "2"]}, + {"0", "0", "1"} + }, + GridBoxAlignment->{ + "Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}}, + "RowsIndexed" -> {}}, + GridBoxSpacings->{"Columns" -> { + Offset[0.27999999999999997`], { + Offset[0.7]}, + Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> { + Offset[0.2], { + Offset[0.4]}, + Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}], + Function[BoxForm`e$, + MatrixForm[BoxForm`e$]]]], "Output", + CellChangeTimes->{{3.627995653969245*^9, 3.627995661346282*^9}, + 3.627996419383007*^9, 3.627996465705708*^9}] +}, Open ]], + +Cell[BoxData[""], "Input", + CellChangeTimes->{{3.627995694633294*^9, 3.627995695945466*^9}}] +}, +WindowSize->{654, 852}, +WindowMargins->{{Automatic, 27}, {Automatic, 0}}, +FrontEndVersion->"8.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (October 5, \ +2011)", +StyleDefinitions->"Default.nb" +] +(* End of Notebook Content *) + +(* Internal cache information *) +(*CellTagsOutline +CellTagsIndex->{} +*) +(*CellTagsIndex +CellTagsIndex->{} +*) +(*NotebookFileOutline +Notebook[{ +Cell[557, 20, 2591, 84, 197, "Text"], +Cell[3151, 106, 2022, 56, 68, "Input"], +Cell[CellGroupData[{ +Cell[5198, 166, 343, 9, 43, "Input"], +Cell[5544, 177, 6519, 190, 290, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[12100, 372, 298, 7, 27, "Input"], +Cell[12401, 381, 2665, 76, 99, "Output"] +}, Open ]], +Cell[15081, 460, 535, 20, 29, "Text"], +Cell[CellGroupData[{ +Cell[15641, 484, 297, 8, 27, "Input"], +Cell[15941, 494, 863, 25, 91, "Output"] +}, Open ]], +Cell[CellGroupData[{ +Cell[16841, 524, 296, 8, 27, "Input"], +Cell[17140, 534, 859, 25, 91, "Output"] +}, Open ]], +Cell[18014, 562, 92, 1, 27, "Input"] +} +] +*) + +(* End of internal cache information *) diff --git a/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 da29b99b6..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 @@ -1224,6 +1211,7 @@ class VectorValues { #include virtual class GaussianFactor { + gtsam::KeyVector keys() const; void print(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; @@ -1647,6 +1635,7 @@ class NonlinearFactorGraph { void push_back(gtsam::NonlinearFactor* factor); void add(gtsam::NonlinearFactor* factor); bool exists(size_t idx) const; + gtsam::KeySet keys() const; // NonlinearFactorGraph double error(const gtsam::Values& values) const; @@ -1733,6 +1722,9 @@ class Values { void insert(size_t j, Vector t); void insert(size_t j, Matrix t); + // Fixed size version + void insertFixed(size_t j, Vector t, size_t n); + void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); void update(size_t j, const gtsam::Rot2& t); @@ -2379,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; @@ -2399,25 +2389,24 @@ class ConstantBias { }///\namespace imuBias #include -class PoseVelocity{ - PoseVelocity(const gtsam::Pose3& pose, Vector velocity); +class PoseVelocityBias{ + PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); }; class ImuFactorPreintegratedMeasurements { // Standard Constructor ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); - ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); + // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); // Testable void print(string s) const; bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); - Matrix measurementCovariance() const; - Matrix deltaRij() const; double deltaTij() const; + Matrix deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; - Vector biasHat() const; + Vector biasHatVector() const; Matrix delPdelBiasAcc() const; Matrix delPdelBiasOmega() const; Matrix delVdelBiasAcc() const; @@ -2425,10 +2414,11 @@ class ImuFactorPreintegratedMeasurements { Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; - // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) const; }; virtual class ImuFactor : gtsam::NonlinearFactor { @@ -2439,11 +2429,60 @@ virtual class ImuFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor); // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, - const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, +}; + +#include +class CombinedImuFactorPreintegratedMeasurements { + // Standard Constructor + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit); + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit, + bool use2ndOrderIntegration); + // CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); + + double deltaTij() const; + Matrix deltaRij() const; + Vector deltaPij() const; + Vector deltaVij() const; + Vector biasHatVector() const; + Matrix delPdelBiasAcc() const; + Matrix delPdelBiasOmega() const; + Matrix delVdelBiasAcc() const; + Matrix delVdelBiasOmega() const; + Matrix delRdelBiasOmega() const; + Matrix preintMeasCov() const; + + // Standard Interface + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); + void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, Vector gravity, Vector omegaCoriolis) const; }; +virtual class CombinedImuFactor : gtsam::NonlinearFactor { + CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, + const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); + // Standard Interface + gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; +}; + #include class AHRSFactorPreintegratedMeasurements { // Standard Constructor @@ -2456,7 +2495,6 @@ class AHRSFactorPreintegratedMeasurements { bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); // get Data - Matrix measurementCovariance() const; Matrix deltaRij() const; double deltaTij() const; Vector biasHat() const; @@ -2483,64 +2521,6 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { Vector omegaCoriolis) const; }; -#include -class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); - }; -class CombinedImuFactorPreintegratedMeasurements { - // Standard Constructor - CombinedImuFactorPreintegratedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit); - CombinedImuFactorPreintegratedMeasurements( - const gtsam::imuBias::ConstantBias& bias, - Matrix measuredAccCovariance, - Matrix measuredOmegaCovariance, - Matrix integrationErrorCovariance, - Matrix biasAccCovariance, - Matrix biasOmegaCovariance, - Matrix biasAccOmegaInit, - bool use2ndOrderIntegration); - CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol); - - // Standard Interface - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); - - Matrix measurementCovariance() const; - Matrix deltaRij() const; - double deltaTij() const; - Vector deltaPij() const; - Vector deltaVij() const; - Vector biasHat() const; - Matrix delPdelBiasAcc() const; - Matrix delPdelBiasOmega() const; - Matrix delVdelBiasAcc() const; - Matrix delVdelBiasOmega() const; - Matrix delRdelBiasOmega() const; - Matrix PreintMeasCov() const; -}; - -virtual class CombinedImuFactor : gtsam::NonlinearFactor { - CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); - - // Standard Interface - gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i, - const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, - Vector gravity, Vector omegaCoriolis); -}; - #include //virtual class AttitudeFactor : gtsam::NonlinearFactor { // AttitudeFactor(const Unit3& nZ, const Unit3& bRef); diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h deleted file mode 100644 index 3cc6a041c..000000000 --- a/gtsam/base/ChartValue.h +++ /dev/null @@ -1,221 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file ChartValue.h - * @brief - * @date October, 2014 - * @author Michael Bosse, Abel Gawel, Renaud Dube - * based on DerivedValue.h by Duy Nguyen Ta - */ - -#pragma once - -#include -#include -#include - -////////////////// -// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR -#include - -#ifdef min -#undef min -#endif - -#ifdef max -#undef max -#endif - -#ifdef ERROR -#undef ERROR -#endif -////////////////// - -namespace gtsam { - -/** - * ChartValue is derived from GenericValue and Chart so that - * Chart can be zero sized (as in DefaultChart) - * if the Chart is a member variable then it won't ever be zero sized. - */ -template > -class ChartValue: public GenericValue, public Chart_ { - - BOOST_CONCEPT_ASSERT((ChartConcept)); - -public: - - typedef T type; - typedef Chart_ Chart; - -public: - - /// Default Constructor. TODO might not make sense for some types - ChartValue() : - GenericValue(T()) { - } - - /// Construct froma value - ChartValue(const T& value) : - GenericValue(value) { - } - - /// Construct from a value and initialize the chart - template - ChartValue(const T& value, C chart_initializer) : - GenericValue(value), Chart(chart_initializer) { - } - - /// Destructor - virtual ~ChartValue() { - } - - /** - * Create a duplicate object returned as a pointer to the generic Value interface. - * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator. - * The result must be deleted with Value::deallocate_, not with the 'delete' operator. - */ - virtual Value* clone_() const { - void *place = boost::singleton_pool::malloc(); - ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in - return ptr; - } - - /** - * Destroy and deallocate this object, only if it was originally allocated using clone_(). - */ - virtual void deallocate_() const { - this->~ChartValue(); // Virtual destructor cleans up the derived object - boost::singleton_pool::free((void*) this); // Release memory from pool - } - - /** - * Clone this value (normal clone on the heap, delete with 'delete' operator) - */ - virtual boost::shared_ptr clone() const { - return boost::make_shared(*this); - } - - /// Chart Value interface version of retract - virtual Value* retract_(const Vector& delta) const { - // Call retract on the derived class using the retract trait function - const T retractResult = Chart::retract(GenericValue::value(), delta); - - // Create a Value pointer copy of the result - void* resultAsValuePlace = - boost::singleton_pool::malloc(); - Value* resultAsValue = new (resultAsValuePlace) ChartValue(retractResult, - static_cast(*this)); - - // Return the pointer to the Value base class - return resultAsValue; - } - - /// Generic Value interface version of localCoordinates - virtual Vector localCoordinates_(const Value& value2) const { - // Cast the base class Value pointer to a templated generic class pointer - const GenericValue& genericValue2 = - static_cast&>(value2); - - // Return the result of calling localCoordinates trait on the derived class - return Chart::local(GenericValue::value(), genericValue2.value()); - } - - /// Non-virtual version of retract - ChartValue retract(const Vector& delta) const { - return ChartValue(Chart::retract(GenericValue::value(), delta), - static_cast(*this)); - } - - /// Non-virtual version of localCoordinates - Vector localCoordinates(const ChartValue& value2) const { - return localCoordinates_(value2); - } - - /// Return run-time dimensionality - virtual size_t dim() const { - // need functional form here since the dimension may be dynamic - return Chart::getDimension(GenericValue::value()); - } - - /// Assignment operator - virtual Value& operator=(const Value& rhs) { - // Cast the base class Value pointer to a derived class pointer - const ChartValue& derivedRhs = static_cast(rhs); - - // Do the assignment and return the result - *this = ChartValue(derivedRhs); // calls copy constructor - return *this; - } - -protected: - - // implicit assignment operator for (const ChartValue& rhs) works fine here - /// Assignment operator, protected because only the Value or DERIVED - /// assignment operators should be used. - // DerivedValue& operator=(const DerivedValue& rhs) { - // // Nothing to do, do not call base class assignment operator - // return *this; - // } - -private: - - /// Fake Tag struct for singleton pool allocator. In fact, it is never used! - struct PoolTag { - }; - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - // ar & boost::serialization::make_nvp("value",); - // todo: implement a serialization for charts - ar - & boost::serialization::make_nvp("GenericValue", - boost::serialization::base_object >(*this)); - } - -}; - -// Define -namespace traits { - -/// The dimension of a ChartValue is the dimension of the chart -template -struct dimension > : public dimension { - // TODO Frank thinks dimension is a property of type, chart should conform -}; - -} // \ traits - -/// Get the chart from a Value -template -const Chart& Value::getChart() const { - return dynamic_cast(*this); -} - -/// Convenience function that can be used to make an expression to convert a value to a chart -template -ChartValue convertToChartValue(const T& value, - boost::optional< - Eigen::Matrix::value, - traits::dimension::value>&> H = boost::none) { - if (H) { - *H = Eigen::Matrix::value, - traits::dimension::value>::Identity(); - } - return ChartValue(value); -} - -} /* namespace gtsam */ diff --git a/gtsam/base/GenericValue.h b/gtsam/base/GenericValue.h index 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 b208a584a..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 dexpL(const Vector& v) { - return eye(1); - } - - /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { - return eye(1); - } + /// @name Lie Group + /// @{ + static Vector1 Logmap(const LieScalar& p) { return p.vector();} + static LieScalar Expmap(const Vector1& v) { return LieScalar(v[0]);} + /// @} private: double d_; }; - // Define GTSAM traits - namespace traits { - template<> - struct is_group : 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/Vector.cpp b/gtsam/base/Vector.cpp index b872aa08b..a9ef8fe10 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -286,11 +286,15 @@ double weightedPseudoinverse(const Vector& a, const Vector& weights, for (size_t i = 0; i < m; ++i) isZero.push_back(fabs(a[i]) < 1e-9); // If there is a valid (a!=0) constraint (sigma==0) return the first one - for (size_t i = 0; i < m; ++i) + for (size_t i = 0; i < m; ++i) { if (weights[i] == inf && !isZero[i]) { + // Basically, instead of doing a normal QR step with the weighted + // pseudoinverse, we enforce the constraint by turning + // ax + AS = b into x + (A/a)S = b/a, for the first row where a!=0 pseudo = delta(m, i, 1 / a[i]); return inf; } + } // Form psuedo-inverse inv(a'inv(Sigma)a)a'inv(Sigma) // For diagonal Sigma, inv(Sigma) = diag(precisions) 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 d2f453521..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; @@ -39,12 +42,6 @@ void testDefaultChart(TestResult& result_, BOOST_CONCEPT_ASSERT((ChartConcept)); T other = value; - // Check for the existence of a print function. - gtsam::traits::print()(value, "value"); - gtsam::traits::print()(other, "other"); - - // Check for the existence of "equals" - EXPECT(gtsam::traits::equals()(value, other, 1e-12)); // Check that the dimension of the local value matches the chart dimension. Vector dx = Chart::local(value, other); 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..e2d0e6aec 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,9 @@ 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 5dabc4bd6..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,66 +107,17 @@ public: /// @{ /// identity - inline static Point2 identity() { - return Point2(); - } + inline static Point2 identity() {return Point2();} - /// "Inverse" - negates each coordinate such that compose(p,inverse(p)) == identity() - inline Point2 inverse() const { return Point2(-x_, -y_); } - - /// syntactic sugar for inverse, i.e., -p == inverse(p) + /// inverse inline Point2 operator- () const {return Point2(-x_,-y_);} - /// "Compose", just adds the coordinates of two points. With optional derivatives - inline Point2 compose(const Point2& q, - 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()); } - /// @} /// @name Vector Space /// @{ @@ -215,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 @@ -245,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 56d9b8bef..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 dexpL(const Vector& v) { - return I_3x3; - } - - /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(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 edc16af03..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 } @@ -124,8 +130,71 @@ Matrix3 Pose2::AdjointMap() const { } /* ************************************************************************* */ -Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const { - if (H1) *H1 = -AdjointMap(); +Matrix3 Pose2::adjointMap(const Vector& v) { + // See Chirikjian12book2, vol.2, pg. 36 + Matrix3 ad = zeros(3,3); + ad(0,1) = -v[2]; + ad(1,0) = v[2]; + ad(0,2) = v[1]; + ad(1,2) = -v[0]; + return ad; +} + +/* ************************************************************************* */ +Matrix3 Pose2::ExpmapDerivative(const Vector3& v) { + double alpha = v[2]; + Matrix3 J; + if (fabs(alpha) > 1e-5) { + // Chirikjian11book2, pg. 36 + /* !!!Warning!!! Compare Iserles05an, formula 2.42 and Chirikjian11book2 pg.26 + * Iserles' right-trivialization dexpR is actually the left Jacobian J_l in Chirikjian's notation + * In fact, Iserles 2.42 can be written as: + * \dot{g} g^{-1} = dexpR_{q}\dot{q} + * where q = A, and g = exp(A) + * and the LHS is in the definition of J_l in Chirikjian11book2, pg. 26. + * Hence, to compute ExpmapDerivative, we have to use the formula of J_r Chirikjian11book2, pg.36 + */ + double sZalpha = sin(alpha)/alpha, c_1Zalpha = (cos(alpha)-1)/alpha; + double v1Zalpha = v[0]/alpha, v2Zalpha = v[1]/alpha; + J << sZalpha, -c_1Zalpha, v1Zalpha + v2Zalpha*c_1Zalpha - v1Zalpha*sZalpha, + c_1Zalpha, sZalpha, -v1Zalpha*c_1Zalpha + v2Zalpha - v2Zalpha*sZalpha, + 0, 0, 1; + } + else { + // Thanks to Krunal: Apply L'Hospital rule to several times to + // compute the limits when alpha -> 0 + J << 1,0,-0.5*v[1], + 0,1, 0.5*v[0], + 0,0, 1; + } + + return J; +} + +/* ************************************************************************* */ +Matrix3 Pose2::LogmapDerivative(const Pose2& p) { + Vector3 v = Logmap(p); + double alpha = v[2]; + Matrix3 J; + if (fabs(alpha) > 1e-5) { + double alphaInv = 1/alpha; + double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); + double v1 = v[0], v2 = v[1]; + + J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, + 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, + 0, 0, 1; + } + else { + J << 1,0, 0.5*v[1], + 0,1, -0.5*v[0], + 0,0, 1; + } + return J; +} + +/* ************************************************************************* */ +Pose2 Pose2::inverse() const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } @@ -143,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, @@ -168,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 a48be51cf..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 @@ -161,6 +133,11 @@ public: return AdjointMap()*xi; } + /** + * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 + */ + static Matrix3 adjointMap(const Vector& v); + /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where @@ -176,6 +153,20 @@ public: return m; } + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& v); + + /// Derivative of Logmap + static Matrix3 LogmapDerivative(const Pose2& v); + + // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP + struct ChartAtOrigin { + static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); + static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); + }; + + using LieGroup::inverse; // version with derivative + /// @} /// @name Group Action on Point2 /// @{ @@ -299,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 0bc07b753..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) @@ -93,26 +97,6 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, return adjointMap(xi).transpose() * y; } -/* ************************************************************************* */ -Matrix6 Pose3::dExpInv_exp(const Vector6& xi) { - // Bernoulli numbers, from Wikipedia - static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, - 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); - static const int N = 5; // order of approximation - Matrix6 res; - res = I_6x6; - Matrix6 ad_i; - ad_i = I_6x6; - Matrix6 ad_xi = adjointMap(xi); - double fac = 1.0; - for (int i = 1; i < N; ++i) { - ad_i = ad_xi * ad_i; - fac = fac * i; - res = res + B(i) / fac * ad_i; - } - return res; -} - /* ************************************************************************* */ void Pose3::print(const string& s) const { cout << s; @@ -127,7 +111,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector& xi) { +Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { + if (H) *H = ExpmapDerivative(xi); // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -147,7 +132,8 @@ Pose3 Pose3::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& p) { +Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { + if (H) *H = LogmapDerivative(p); Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = w.norm(); if (t < 1e-10) { @@ -168,57 +154,104 @@ Vector6 Pose3::Logmap(const Pose3& p) { } /* ************************************************************************* */ -Pose3 Pose3::retractFirstOrder(const Vector& xi) const { - Vector3 omega(sub(xi, 0, 3)); - Point3 v(sub(xi, 3, 6)); - Rot3 R = R_.retract(omega); // R is done exactly - Point3 t = t_ + R_ * v; // First order t approximation - return Pose3(R, t); +Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, H); +#else + Matrix3 DR; + Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0); + if (H) { + *H = I_6x6; + H->topLeftCorner<3,3>() = DR; + } + return Pose3(R, Point3(xi.tail<3>())); +#endif } /* ************************************************************************* */ -// Different versions of retract -Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group exponential map, traces out geodesic - return compose(Expmap(xi)); - } else if (mode == Pose3::FIRST_ORDER) { - // First order - return retractFirstOrder(xi); - } else { - // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently - // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation - assert(false); - exit(1); +Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(T, H); +#else + Matrix3 DR; + Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); + if (H) { + *H = I_6x6; + H->topLeftCorner<3,3>() = DR; } + Vector6 xi; + xi << omega, T.translation().vector(); + return xi; +#endif } /* ************************************************************************* */ -// different versions of localCoordinates -Vector6 Pose3::localCoordinates(const Pose3& T, - Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group logarithm map, exact inverse of exponential map - return Logmap(between(T)); - } else if (mode == Pose3::FIRST_ORDER) { - // R is always done exactly in all three retract versions below - Vector3 omega = R_.localCoordinates(T.rotation()); +/** + * Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix + * J(xi) = [J_(w) Z_3x3; + * Q J_(w)] + * where J_(w) is the SO3 Expmap derivative. + * (see Chirikjian11book2, pg 44, eq 10.95. + * The closed-form formula is similar to formula 102 in Barfoot14tro) + */ +static Matrix3 computeQforExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Vector3 v(sub(xi, 3, 6)); + Matrix3 V = skewSymmetric(v); + Matrix3 W = skewSymmetric(w); - // Incorrect version - // Independently computes the logmap of the translation and rotation - // Vector v = t_.localCoordinates(T.translation()); + Matrix3 Q; - // Correct first order t inverse - Point3 d = R_.unrotate(T.translation() - t_); - - // TODO: correct second order t inverse - Vector6 local; - local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z(); - return local; - } else { - assert(false); - exit(1); +#ifdef NUMERICAL_EXPMAP_DERIV + Matrix3 Qj = Z_3x3; + double invFac = 1.0; + Q = Z_3x3; + Matrix3 Wj = I_3x3; + for (size_t j=1; j<10; ++j) { + Qj = Qj*W + Wj*V; + invFac = -invFac/(j+1); + Q = Q + invFac*Qj; + Wj = Wj*W; } +#else + // The closed-form formula in Barfoot14tro eq. (102) + double phi = w.norm(); + if (fabs(phi)>1e-5) { + double sinPhi = sin(phi), cosPhi = cos(phi); + double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); + } + else { + Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) + + 1./24.*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + } +#endif + + return Q; +} + +/* ************************************************************************* */ +Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::ExpmapDerivative(w); + Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished(); + return J; +} + +/* ************************************************************************* */ +Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { + Vector6 xi = Logmap(pose); + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::LogmapDerivative(w); + Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix3 Q2 = -Jw*Q*Jw; + Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished(); + return J; } /* ************************************************************************* */ @@ -271,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 6a7ed8456..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,243 +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); - - /** - * Compute the inverse right-trivialized tangent (derivative) map of the exponential map, - * as detailed in [Kobilarov09siggraph] eq. (15) - * The full formula is documented in [Celledoni99cmame] - * Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and - * time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421-438, 2003. - * and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2 - * Ernst Hairer, et al., Geometric Numerical Integration, - * Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006. - */ - static Matrix6 dExpInv_exp(const 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)); @@ -354,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 d5ea6afdc..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,70 +97,48 @@ namespace gtsam { inline static Rot2 identity() { return Rot2(); } /** The inverse rotation - negative angle */ - Rot2 inverse() const { - return Rot2(c_, -s_); - } - - /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R, 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 Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none); + + /** Calculate Adjoint map */ + Matrix1 AdjointMap() const { return I_1x1; } + + /// Left-trivialized derivative of the exponential map + static Matrix ExpmapDerivative(const Vector& v) { + return ones(1); } - ///Log map at identity - return the canonical coordinates of this rotation - static inline Vector1 Logmap(const Rot2& r) { - Vector1 v; - v << r.theta(); - return v; + /// Left-trivialized derivative inverse of the exponential map + static Matrix LogmapDerivative(const Vector& v) { + return ones(1); } + // Chart at origin simply uses exponential map and its inverse + struct ChartAtOrigin { + static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) { + return Expmap(v, H); + } + static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) { + return Logmap(r, H); + } + }; + + using LieGroup::inverse; // version with derivative + /// @} /// @name Group Action on Point2 /// @{ @@ -243,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 0b88a70c7..ab44716c8 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -107,32 +107,6 @@ Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, return q; } -/* ************************************************************************* */ -/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix3 x = skewSymmetric(v); - Matrix3 x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2; - return res; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpInvL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix3 x = skewSymmetric(v); - Matrix3 x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix3 res = I_3x3 + 0.5*x - s2*x2; - return res; -} - - /* ************************************************************************* */ Point3 Rot3::column(int index) const{ if(index == 3) @@ -175,36 +149,57 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = I_3x3; - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; +Matrix3 Rot3::ExpmapDerivative(const Vector3& x) { + if(zero(x)) return I_3x3; + double theta = x.norm(); // rotation angle +#ifdef DUY_VERSION + /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(x); + Matrix3 X2 = X*X; + double vi = theta/2.0; + double s1 = sin(vi)/vi; + double s2 = (theta - sin(theta))/(theta*theta*theta); + return I_3x3 - 0.5*s1*s1*X + s2*X2; +#else // Luca's version + /** + * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) + * where Jr = ExpmapDerivative(thetahat); + * This maps a perturbation in the tangent space (omega) to + * a perturbation on the manifold (expmap(Jr * omega)) + */ + // element of Lie algebra so(3): X = x^, normalized by normx + const Matrix3 Y = skewSymmetric(x) / theta; + return I_3x3 - ((1 - cos(theta)) / (theta)) * Y + + (1 - sin(theta) / theta) * Y * Y; // right Jacobian +#endif } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = I_3x3; - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = I_3x3 + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; +Matrix3 Rot3::LogmapDerivative(const Vector3& x) { + if(zero(x)) return I_3x3; + double theta = x.norm(); +#ifdef DUY_VERSION + /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(x); + Matrix3 X2 = X*X; + double vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + return I_3x3 + 0.5*X - s2*X2; +#else // Luca's version + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega + * where Jrinv = LogmapDerivative(omega); + * This maps a perturbation on the manifold (expmap(omega)) + * to a perturbation in the tangent space (Jrinv * omega) + */ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + return I_3x3 + 0.5 * X + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X + * X; +#endif } /* ************************************************************************* */ @@ -239,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 6fe3f92b4..e9568fb26 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -16,11 +16,15 @@ * @author Christian Potthast * @author Frank Dellaert * @author Richard Roberts + * @author Luca Carlone */ // \callgraph #pragma once +#include +#include +#include #include // Get GTSAM_USE_QUATERNIONS macro // You can override the default coordinate mode using this flag @@ -39,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 @@ -58,7 +52,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot3 { + class GTSAM_EXPORT Rot3 : public LieGroup { private: @@ -160,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 @@ -214,16 +208,13 @@ namespace gtsam { return Rot3(); } - /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(boost::optional 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 @@ -234,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 @@ -264,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 /// @{ @@ -287,32 +273,33 @@ namespace gtsam { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula */ - static Rot3 Expmap(const Vector& v) { - if(zero(v)) return Rot3(); - else return rodriguez(v); + static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { + if(H) *H = Rot3::ExpmapDerivative(v); + if (zero(v)) return Rot3(); else return rodriguez(v); } /** - * Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const Rot3& R); + static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none); - /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector3& v); + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& x); - /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector3& v); + /// Derivative of Logmap + static Matrix3 LogmapDerivative(const Vector3& x); - /** - * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x); + /** Calculate Adjoint map */ + Matrix3 AdjointMap() const { return matrix(); } - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); + // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE + struct ChartAtOrigin { + static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none); + static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none); + }; + + using LieGroup::inverse; // version with derivative /// @} /// @name Group Action on Point3 @@ -474,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 fc3490fb5..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(boost::optional 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 { @@ -184,7 +162,7 @@ Point3 Rot3::rotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R) { +Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { static const double PI = boost::math::constants::pi(); @@ -192,6 +170,8 @@ Vector3 Rot3::Logmap(const Rot3& R) { // Get trace(R) double tr = rot.trace(); + Vector3 thetaR; + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (std::abs(tr+1.0) < 1e-10) { @@ -202,7 +182,7 @@ Vector3 Rot3::Logmap(const Rot3& R) { return (PI / sqrt(2.0+2.0*rot(1,1))) * Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*rot(0,0))) * + thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) * Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); } else { double magnitude; @@ -215,68 +195,60 @@ Vector3 Rot3::Logmap(const Rot3& R) { // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) magnitude = 0.5 - tr_3*tr_3/12.0; } - return magnitude*Vector3( + thetaR = magnitude*Vector3( rot(2,1)-rot(1,2), rot(0,2)-rot(2,0), rot(1,0)-rot(0,1)); } + + if(H) *H = Rot3::LogmapDerivative(thetaR); + return thetaR; } /* ************************************************************************* */ -Rot3 Rot3::retractCayley(const Vector& omega) const { +Rot3 Rot3::CayleyChart::Retract(const Vector3& omega, OptionalJacobian<3,3> H) { + if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative"); const double x = omega(0), y = omega(1), z = omega(2); const double x2 = x * x, y2 = y * y, z2 = z * z; const double xy = x * y, xz = x * z, yz = y * z; const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; - return (*this) - * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, + return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); } /* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return (*this)*Expmap(omega); - } else if(mode == Rot3::CAYLEY) { - return retractCayley(omega); - } else if(mode == Rot3::SLOW_CAYLEY) { - 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 26ca25bf2..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, @@ -101,7 +102,7 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::inverse(boost::optional H1) const { + Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { if (H1) *H1 = -matrix(); return Rot3(quaternion_.inverse()); } @@ -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,31 +134,9 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R) { - using std::acos; - using std::sqrt; - static const double twoPi = 2.0 * M_PI, - // define these compile time constants to avoid std::abs: - NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; - - const Quaternion& q = R.quaternion_; - const double qw = q.w(); - if (qw > NearlyOne) { - // Taylor expansion of (angle / s) at 1 - return (2 - 2 * (qw - 1) / 3) * q.vec(); - } else if (qw < NearlyNegativeOne) { - // Angle is zero, return zero vector - return Vector3::Zero(); - } else { - // Normal, away from zero case - double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); - // Important: convert to [-pi,pi] to keep error continuous - if (angle > M_PI) - angle -= twoPi; - else if (angle < -M_PI) - angle += twoPi; - return (angle / s) * q.vec(); - } + Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { + if(H) *H = Rot3::LogmapDerivative(thetaR); + return QuaternionChart::Logmap(R.quaternion_); } /* ************************************************************************* */ 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 f12d96899..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,24 +143,22 @@ 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 { double w=0.3; - Vector xi = (Vector(3) << 0.0, w, w); + Vector xi = (Vector(3) << 0.0, w, w).finished(); Rot2 expectedR = Rot2::fromAngle(w); Point2 expectedT(-0.0446635, 0.29552); 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)); @@ -192,6 +195,48 @@ TEST(Pose2, logmap_full) { EXPECT(assert_equal(expected, actual, 1e-5)); } +/* ************************************************************************* */ +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, 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)); +} + +/* ************************************************************************* */ +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( 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)); +} + /* ************************************************************************* */ static Point2 transform_to_proxy(const Pose2& pose, const Point2& point) { return pose.transform_to(point); @@ -482,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 @@ -729,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 52f721f41..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,25 +678,28 @@ TEST(Pose3, align_2) { } /* ************************************************************************* */ -/// exp(xi) exp(y) = exp(xi + x) -/// Hence, y = log (exp(-xi)*exp(xi+x)) -Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0).finished(); - -Vector testDerivExpmapInv(const Vector6& dxi) { - return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); -} - -TEST( Pose3, dExpInv_TLN) { - Matrix res = Pose3::dExpInv_exp(xi); - - Matrix numericalDerivExpmapInv = numericalDerivative11( - testDerivExpmapInv, Vector6::Zero(), 1e-5); - - EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); +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; } @@ -700,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)); @@ -708,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; } @@ -720,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)); @@ -736,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 cfb103c5d..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,6 +155,47 @@ TEST( Rot2, relativeBearing ) CHECK(assert_equal(expectedH,actualH)); } +//****************************************************************************** +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); + +} + +//****************************************************************************** +TEST(Rot2 , LieGroupDerivatives) { + Rot2 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(Rot2 , ChartDerivatives) { + Rot2 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/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index c89d2dacb..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,33 +219,94 @@ TEST(Rot3, log) CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) } -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +/* ************************************************************************* */ +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, rightJacobianExpMapSO3 ) +TEST(Rot3, retract_localCoordinates2) { - // Linearization point - Vector3 thetahat; thetahat << 0.1, 0, 0; + 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); - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1), thetahat); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); - CHECK(assert_equal(expectedJacobian, actualJacobian)); +// Left trivialization Derivative of exp(w) wrpt w: +// How does exp(w) change when w changes? +// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// => y = log (exp(-w) * exp(w+dw)) +Vector3 testDexpL(const Vector3& dw) { + return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); +} + +TEST( Rot3, ExpmapDerivative) { + Matrix actualDexpL = Rot3::ExpmapDerivative(w); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); + + Matrix actualDexpInvL = Rot3::LogmapDerivative(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); } /* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3inverse ) +Vector3 thetahat(0.1, 0, 0.1); +TEST( Rot3, ExpmapDerivative2) { - // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias - Vector3 deltatheta; deltatheta << 0, 0, 0; + Matrix Jexpected = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1, boost::none), thetahat); - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); - EXPECT(assert_equal(expectedJacobian, actualJacobian)); + Matrix Jactual = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual)); + + Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat); + CHECK(assert_equal(Jexpected, Jactual2)); +} + +/* ************************************************************************* */ +TEST( Rot3, jacobianExpmap ) +{ + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Expmap, _1, boost::none), thetahat); + Matrix3 Jactual; + const Rot3 R = Rot3::Expmap(thetahat, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST( Rot3, LogmapDerivative ) +{ + Rot3 R = Rot3::Expmap(thetahat); // some rotation + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Logmap, _1, boost::none), R); + Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST( Rot3, jacobianLogmap ) +{ + Rot3 R = Rot3::Expmap(thetahat); // some rotation + Matrix Jexpected = numericalDerivative11(boost::bind( + &Rot3::Logmap, _1, boost::none), R); + Matrix3 Jactual; + Rot3::Logmap(R, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); } /* ************************************************************************* */ @@ -251,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))); @@ -400,27 +464,6 @@ TEST( Rot3, between ) CHECK(assert_equal(numericalH2,actualH2)); } -/* ************************************************************************* */ -Vector w = Vector3(0.1, 0.27, -0.2); - -// Left trivialization Derivative of exp(w) wrpt w: -// How does exp(w) change when w changes? -// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector3 testDexpL(const Vector3& dw) { - return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); -} - -TEST( Rot3, dexpL) { - Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, - Vector3::Zero(), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); - - Matrix actualDexpInvL = Rot3::dexpInvL(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); -} - /* ************************************************************************* */ TEST( Rot3, xyz ) { @@ -574,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); @@ -602,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.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 0505f6c01..5e7e08f61 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -2,7 +2,7 @@ * ConjugateGradientSolver.cpp * * Created on: Jun 4, 2014 - * Author: ydjian + * Author: Yong-Dian Jian */ #include diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 6e8509309..20e0c5262 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -9,6 +9,14 @@ * -------------------------------------------------------------------------- */ +/** + * @file ConjugateGradientSolver.h + * @brief Implementation of Conjugate Gradient solver for a linear system + * @author Yong-Dian Jian + * @author Sungtae An + * @date Nov 6, 2014 + **/ + #pragma once #include @@ -82,9 +90,13 @@ public: /*********************************************************************************************/ /* * A template of linear preconditioned conjugate gradient method. - * System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v, - * rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. + * + ** REFERENCES: + * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, + * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. */ template V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { @@ -93,8 +105,8 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju estimate = residual = direction = q1 = q2 = initial; system.residual(estimate, q1); /* q1 = b-Ax */ - system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ - system.rightPrecondition(residual, direction);/* d = S^{-1} r */ + system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ + system.rightPrecondition(residual, direction);/* p = L^{-T} r */ double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta; @@ -116,27 +128,29 @@ V preconditionedConjugateGradient(const S &system, const V &initial, const Conju if ( k % iReset == 0 ) { system.residual(estimate, q1); /* q1 = b-Ax */ - system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ - system.rightPrecondition(residual, direction); /* d = S^{-1} r */ + system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ + system.rightPrecondition(residual, direction); /* p = L^{-T} r */ currentGamma = system.dot(residual, residual); } - system.multiply(direction, q1); /* q1 = A d */ - alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */ - system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */ - system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */ - system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */ + system.multiply(direction, q1); /* q1 = A p */ + alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */ + system.axpy(alpha, direction, estimate); /* estimate += alpha * p */ + system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */ + system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */ prevGamma = currentGamma; - currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */ + currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */ beta = currentGamma / prevGamma; - system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */ + system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */ system.scal(beta, direction); - system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */ + system.axpy(1.0, q1, direction); /* p = q1 + beta * p */ if (parameters.verbosity() >= ConjugateGradientParameters::ERROR ) std::cout << "[PCG] k = " << k << ", 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 8c8e2cb2b..16b1f759a 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -20,8 +20,9 @@ #pragma once -#include #include +#include +#include namespace gtsam { @@ -133,6 +134,9 @@ namespace gtsam { /// A'*b for Jacobian, eta for Hessian (raw memory version) virtual void gradientAtZero(double* d) const = 0; + /// Gradient wrt a key at any values + virtual Vector gradient(Key key, const VectorValues& x) const = 0; + private: /** Serialization function */ friend class boost::serialization::access; @@ -143,4 +147,9 @@ namespace gtsam { }; // GaussianFactor -} // namespace gtsam +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 54e721cd7..2879e0a4c 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -254,6 +254,7 @@ namespace gtsam { map GaussianFactorGraph::hessianBlockDiagonal() const { map blocks; BOOST_FOREACH(const sharedFactor& factor, *this) { + if (!factor) continue; map BD = factor->hessianBlockDiagonal(); map::const_iterator it = BD.begin(); for(;it!=BD.end();it++) { @@ -303,6 +304,7 @@ namespace gtsam { // Zero-out the gradient VectorValues g; BOOST_FOREACH(const sharedFactor& factor, *this) { + if (!factor) continue; VectorValues gi = factor->gradientAtZero(); g.addInPlace_(gi); } @@ -393,15 +395,15 @@ namespace gtsam { /* ************************************************************************* */ // x += alpha*A'*e -void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, - VectorValues& x) const { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Ai->transposeMultiplyAdd(alpha, *(ei++), x); + void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorValues& x) const { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha, *(ei++), x); + } } -} ///* ************************************************************************* */ //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { 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.cpp b/gtsam/linear/HessianFactor.cpp index f282682b3..6d7f3c2e1 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -616,6 +616,30 @@ void HessianFactor::gradientAtZero(double* d) const { } } +/* ************************************************************************* */ +Vector HessianFactor::gradient(Key key, const VectorValues& x) const { + Factor::const_iterator i = find(key); + // Sum over G_ij*xj for all xj connecting to xi + Vector b = zero(x.at(key).size()); + for (Factor::const_iterator j = begin(); j != end(); ++j) { + // Obtain Gij from the Hessian factor + // Hessian factor only stores an upper triangular matrix, so be careful when i>j + Matrix Gij; + if (i > j) { + Matrix Gji = info(j, i); + Gij = Gji.transpose(); + } + else { + Gij = info(i, j); + } + // Accumulate Gij*xj to gradf + b += Gij * x.at(*j); + } + // Subtract the linear term gi + b += -linearTerm(i); + return b; +} + /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 62d84925c..ec4710dd7 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -389,6 +389,12 @@ namespace gtsam { virtual void gradientAtZero(double* d) const; + /** + * Compute the gradient at a key: + * \grad f(x_i) = \sum_j G_ij*x_j - g_i + */ + Vector gradient(Key key, const VectorValues& x) const; + /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor @@ -437,6 +443,11 @@ namespace gtsam { } }; -} +/// traits +template<> +struct traits : public Testable {}; + +} // \ namespace gtsam + #include diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index a63bbf473..566a98fc2 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -575,7 +575,14 @@ VectorValues JacobianFactor::gradientAtZero() const { /* ************************************************************************* */ void JacobianFactor::gradientAtZero(double* d) const { - //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); + throw std::runtime_error("Raw memory version of gradientAtZero not implemented for Jacobian factor"); +} + +/* ************************************************************************* */ +Vector JacobianFactor::gradient(Key key, const VectorValues& x) const { + // TODO: optimize it for JacobianFactor without converting to a HessianFactor + HessianFactor hessian(*this); + return hessian.gradient(key, x); } /* ************************************************************************* */ diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 92f43b6be..4b44197c6 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -230,7 +230,9 @@ namespace gtsam { virtual bool empty() const { return size() == 0 /*|| rows() == 0*/; } /** is noise model constrained ? */ - bool isConstrained() const { return model_->isConstrained(); } + bool isConstrained() const { + return model_ && model_->isConstrained(); + } /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables? @@ -288,9 +290,12 @@ namespace gtsam { /// A'*b for Jacobian VectorValues gradientAtZero() const; - /* ************************************************************************* */ + /// A'*b for Jacobian (raw memory version) virtual void gradientAtZero(double* d) const; + /// Compute the gradient wrt a key at any values + Vector gradient(Key key, const VectorValues& x) const; + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; @@ -355,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/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 27eb57b44..ffb744239 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -2,20 +2,14 @@ * PCGSolver.cpp * * Created on: Feb 14, 2012 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #include -//#include -//#include -//#include -//#include #include #include -//#include -//#include -//#include -//#include + #include #include #include @@ -33,6 +27,7 @@ void PCGSolverParameters::print(ostream &os) const { /*****************************************************************************/ PCGSolver::PCGSolver(const PCGSolverParameters &p) { + parameters_ = p; preconditioner_ = createPreconditioner(p.preconditioner_); } @@ -76,97 +71,47 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { } /*****************************************************************************/ -void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const { - /* implement Ax, assume x and Ax are pre-allocated */ +void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const { + /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ - /* reset y */ - Ax.setZero(); + // Build a VectorValues for Vector x + VectorValues vvX = buildVectorValues(x,keyInfo_); - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate At A x*/ - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - const Matrix Ai = jf->getA(it); - /* this map lookup should be replaced */ - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - Ax.segment(entry.colstart(), entry.dim()) - += Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim())); - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate H x */ + // VectorValues form of A'Ax for multiplyHessianAdd + VectorValues vvAtAx; - /* use buffer to avoid excessive table lookups */ - const size_t sz = hf->size(); - vector y; - y.reserve(sz); - for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { - /* initialize y to zeros */ - y.push_back(zero(hf->getDim(it))); - } + // vvAtAx += 1.0 * A'Ax for each factor + gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); - for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) { - /* retrieve the key mapping */ - const KeyInfoEntry &entry = keyInfo_.find(*j)->second; - // xj is the input vector - const Vector xj = x.segment(entry.colstart(), entry.dim()); - size_t idx = 0; - for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) { - if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj; - else y[idx] += hf->info(i, j).knownOffDiagonal() * xj; - } - } + // Make the result as Vector form + AtAx = vvAtAx.vector(); - /* accumulate to r */ - for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) { - /* retrieve the key mapping */ - const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second; - Ax.segment(entry.colstart(), entry.dim()) += y[i]; - } - } - else { - throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } } /*****************************************************************************/ void GaussianFactorGraphSystem::getb(Vector &b) const { /* compute rhs, assume b pre-allocated */ - /* reset */ - b.setZero(); + // Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues + VectorValues vvb = gfg_.gradientAtZero(); - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - const Vector rhs = jf->getb(); - /* accumulate At rhs */ - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - /* this map lookup should be replaced */ - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ; - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - /* accumulate g */ - for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { - const KeyInfoEntry &entry = keyInfo_.find(*it)->second; - b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it); - } - } - else { - throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } + // Make the result as Vector form + b = -vvb.vector(); } /**********************************************************************************/ -void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.transposeSolve(x, y); } +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const { + // For a preconditioner M = L*L^T + // Calculate y = L^{-1} x + preconditioner_.solve(x, y); +} /**********************************************************************************/ -void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const -{ preconditioner_.solve(x, y); } +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const { + // For a preconditioner M = L*L^T + // Calculate y = L^{-T} x + preconditioner_.transposeSolve(x, y); +} /**********************************************************************************/ VectorValues buildVectorValues(const Vector &v, diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp index 9af362fba..4c3006a3f 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -2,7 +2,8 @@ * Preconditioner.cpp * * Created on: Jun 2, 2014 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #include @@ -94,7 +95,7 @@ void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const { const Eigen::Map R(ptr, d, d); Eigen::Map b(dst, d, 1); - R.triangularView().solveInPlace(b); + R.triangularView().solveInPlace(b); dst += d; ptr += sz; @@ -141,11 +142,9 @@ void BlockJacobiPreconditioner::build( } /* getting the block diagonals over the factors */ - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - std::map hessianMap = gf->hessianBlockDiagonal(); - BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) - blocks.push_back(hessian); - } + std::map hessianMap =gfg.hessianBlockDiagonal(); + BOOST_FOREACH ( const Matrix hessian, hessianMap | boost::adaptors::map_values) + blocks.push_back(hessian); /* if necessary, allocating the memory for cacheing the factorization results */ if ( nnz > bufferSize_ ) { @@ -159,11 +158,12 @@ void BlockJacobiPreconditioner::build( double *ptr = buffer_; for ( size_t i = 0 ; i < n ; ++i ) { /* use eigen to decompose Di */ - const Matrix R = blocks[i].llt().matrixL().transpose(); + /* It is same as L = chol(M,'lower') in MATLAB where M is full preconditioner */ + const Matrix L = blocks[i].llt().matrixL(); /* store the data in the buffer */ size_t sz = dims_[i]*dims_[i] ; - std::copy(R.data(), R.data() + sz, ptr); + std::copy(L.data(), L.data() + sz, ptr); /* advance the pointer */ ptr += sz; diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 10ceb78a9..623b29863 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -2,7 +2,8 @@ * Preconditioner.h * * Created on: Jun 2, 2014 - * Author: ydjian + * Author: Yong-Dian Jian + * Author: Sungtae An */ #pragma once @@ -57,7 +58,8 @@ struct GTSAM_EXPORT PreconditionerParameters { }; /* PCG aims to solve the problem: A x = b by reparametrizing it as - * S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M + * L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b, + * where A \approx L L^{T}, or A \approx M * The goal of this class is to provide a general interface to all preconditioners */ class GTSAM_EXPORT Preconditioner { public: @@ -70,15 +72,15 @@ public: /* Computation Interfaces */ - /* implement x = S^{-1} y */ + /* implement x = L^{-1} y */ virtual void solve(const Vector& y, Vector &x) const = 0; // virtual void solve(const VectorValues& y, VectorValues &x) const = 0; - /* implement x = S^{-T} y */ + /* implement x = L^{-T} y */ virtual void transposeSolve(const Vector& y, Vector& x) const = 0; // virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; -// /* implement x = S^{-1} S^{-T} y */ +// /* implement x = L^{-1} L^{-T} y */ // virtual void fullSolve(const Vector& y, Vector &x) const = 0; // virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; 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/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index cba65580e..3a2cd0fd4 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -449,6 +449,32 @@ TEST(HessianFactor, gradientAtZero) EXPECT(assert_equal(expectedG, actualG)); } +/* ************************************************************************* */ +TEST(HessianFactor, gradient) +{ + Matrix G11 = (Matrix(1, 1) << 1).finished(); + Matrix G12 = (Matrix(1, 2) << 0, 0).finished(); + Matrix G22 = (Matrix(2, 2) << 1, 0, 0, 1).finished(); + Vector g1 = (Vector(1) << -7).finished(); + Vector g2 = (Vector(2) << -8, -9).finished(); + double f = 194; + + HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); + + // test gradient + Vector x0 = (Vector(1) << 3.0).finished(); + Vector x1 = (Vector(2) << -3.5, 7.1).finished(); + VectorValues x = pair_list_of(0, x0) (1, x1); + + Vector expectedGrad0 = (Vector(1) << 10.0).finished(); + Vector expectedGrad1 = (Vector(2) << 4.5, 16.1).finished(); + Vector grad0 = factor.gradient(0, x); + Vector grad1 = factor.gradient(1, x); + + EXPECT(assert_equal(expectedGrad0, grad0)); + EXPECT(assert_equal(expectedGrad1, grad1)); +} + /* ************************************************************************* */ TEST(HessianFactor, hessianDiagonal) { diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 137f102b3..2f03d72a4 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -18,9 +18,9 @@ **/ #include +#include -/* External or standard includes */ -#include +using namespace std; namespace gtsam { @@ -29,47 +29,35 @@ namespace gtsam { //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( const Vector3& bias, const Matrix3& measuredOmegaCovariance) : - biasHat_(bias), deltaTij_(0.0) { - measurementCovariance_ << measuredOmegaCovariance; - delRdelBiasOmega_.setZero(); + PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias) +{ preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : - biasHat_(Vector3()), deltaTij_(0.0) { - measurementCovariance_.setZero(); - delRdelBiasOmega_.setZero(); - delRdelBiasOmega_.setZero(); + PreintegratedRotation(I_3x3), biasHat_(Vector3()) +{ preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ -void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { - std::cout << s << std::endl; - std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl; - deltaRij_.print(" deltaRij "); - std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" - << std::endl; - std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl; +void AHRSFactor::PreintegratedMeasurements::print(const string& s) const { + PreintegratedRotation::print(s); + cout << "biasHat [" << biasHat_.transpose() << "]" << endl; + cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool AHRSFactor::PreintegratedMeasurements::equals( const PreintegratedMeasurements& other, double tol) const { - return equal_with_abs_tol(biasHat_, other.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, - other.measurementCovariance_, tol) - && deltaRij_.equals(other.deltaRij_, tol) - && std::fabs(deltaTij_ - other.deltaTij_) < tol - && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); + return PreintegratedRotation::equals(other, tol) + && equal_with_abs_tol(biasHat_, other.biasHat_, tol); } //------------------------------------------------------------------------------ void AHRSFactor::PreintegratedMeasurements::resetIntegration() { - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delRdelBiasOmega_.setZero(); + PreintegratedRotation::resetIntegration(); preintMeasCov_.setZero(); } @@ -78,7 +66,6 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredOmega, double deltaT, boost::optional body_P_sensor) { - // NOTE: order is important here because each update uses old values. // First we compensate the measurements for the bias Vector3 correctedOmega = measuredOmega - biasHat_; @@ -93,64 +80,27 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement( // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 theta_incr = correctedOmega * deltaT; + Matrix3 D_Rincr_integratedOmega; + const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !! - // rotation increment computed from the current rotation rate measurement - const Rot3 incrR = Rot3::Expmap(theta_incr); - const Matrix3 incrRt = incrR.transpose(); + // Update Jacobian + update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT); - // Right Jacobian computed at theta_incr - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); - - // Update Jacobians - // --------------------------------------------------------------------------- - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT; - - // Update preintegrated measurements covariance - // --------------------------------------------------------------------------- - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i); - - Rot3 Rot_j = deltaRij_ * incrR; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // Parameterization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - // Update preintegrated measurements covariance: as in [2] we consider a first - // order propagation that can be seen as a prediction phase in an EKF framework - Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11 - // (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrpt preintegrated measurements (df/dx) - const Matrix3& F = H_angles_angles; + // Update rotation and deltaTij. + Matrix3 Fr; // Jacobian of the update + updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr); // first order uncertainty propagation // the deltaT allows to pass from continuous time noise to discrete time noise - preintMeasCov_ = F * preintMeasCov_ * F.transpose() - + measurementCovariance_ * deltaT; - - // Update preintegrated measurements - // --------------------------------------------------------------------------- - deltaRij_ = deltaRij_ * incrR; - deltaTij_ += deltaT; + preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + + gyroscopeCovariance() * deltaT; } //------------------------------------------------------------------------------ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, boost::optional H) const { const Vector3 biasOmegaIncr = bias - biasHat_; - Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr; - const Rot3 deltaRij_biascorrected = deltaRij_.retract( - delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP); - const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - if (H) { - const Matrix3 Jrinv_theta_bc = // - Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = // - Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr); - (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; - } - return theta_biascorrected; + return biascorrectedThetaRij(biasOmegaIncr, H); } //------------------------------------------------------------------------------ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( @@ -172,7 +122,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles( // AHRSFactor methods //------------------------------------------------------------------------------ AHRSFactor::AHRSFactor() : - preintegratedMeasurements_(Vector3(), Matrix3::Zero()) { + _PIM_(Vector3(), Z_3x3) { } AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, @@ -180,7 +130,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, const Vector3& omegaCoriolis, boost::optional body_P_sensor) : Base( noiseModel::Gaussian::Covariance( - preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( + preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_( preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( body_P_sensor) { } @@ -192,13 +142,12 @@ gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const { } //------------------------------------------------------------------------------ -void AHRSFactor::print(const std::string& s, +void AHRSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," + cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","; - preintegratedMeasurements_.print(" preintegrated measurements:"); - std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" - << std::endl; + _PIM_.print(" preintegrated measurements:"); + cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; noiseModel_->print(" noise model: "); if (body_P_sensor_) body_P_sensor_->print(" sensor pose in body frame: "); @@ -207,8 +156,7 @@ void AHRSFactor::print(const std::string& s, //------------------------------------------------------------------------------ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { const This *e = dynamic_cast(&other); - return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) + return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ @@ -216,50 +164,49 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const { } //------------------------------------------------------------------------------ -Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, +Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj, const Vector3& bias, boost::optional H1, boost::optional H2, boost::optional H3) const { // Do bias correction, if (H3) will contain 3*3 derivative used below - const Vector3 theta_biascorrected = // - preintegratedMeasurements_.predict(bias, H3); + const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3); // Coriolis term - const Vector3 coriolis = // - preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_); - const Vector3 theta_corrected = theta_biascorrected - coriolis; + const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_); + const Matrix3 coriolisHat = skewSymmetric(coriolis); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; // Prediction - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); // Get error between actual and prediction - const Rot3 actualRij = rot_i.between(rot_j); - const Rot3 fRhat = deltaRij_corrected.between(actualRij); - Vector3 fR = Rot3::Logmap(fRhat); + const Rot3 actualRij = Ri.between(Rj); + const Rot3 fRrot = correctedDeltaRij.between(actualRij); + Vector3 fR = Rot3::Logmap(fRrot); // Terms common to derivatives - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR); + const Matrix3 D_cDeltaRij_cOmega = Rot3::ExpmapDerivative(correctedOmega); + const Matrix3 D_fR_fRrot = Rot3::LogmapDerivative(fR); if (H1) { // dfR/dRi H1->resize(3, 3); - Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); + Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat; (*H1) - << Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta); + << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis); } if (H2) { // dfR/dPosej H2->resize(3, 3); - (*H2) << Jrinv_fRhat * Matrix3::Identity(); + (*H2) << D_fR_fRrot * Matrix3::Identity(); } if (H3) { // dfR/dBias, note H3 contains derivative of predict - const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3); + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * (*H3); H3->resize(3, 3); - (*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega); + (*H3) << D_fR_fRrot * (-fRrot.transpose() * JbiasOmega); } Vector error(3); @@ -272,16 +219,16 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, const PreintegratedMeasurements preintegratedMeasurements, const Vector3& omegaCoriolis, boost::optional body_P_sensor) { - const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias); + const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias); // Coriolis term const Vector3 coriolis = // preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis); - const Vector3 theta_corrected = theta_biascorrected - coriolis; - const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected); + const Vector3 correctedOmega = biascorrectedOmega - coriolis; + const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); - return rot_i.compose(deltaRij_corrected); + return rot_i.compose(correctedDeltaRij); } } //namespace gtsam diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index e62a24cca..fd4b9be87 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -20,6 +20,7 @@ #pragma once /* GTSAM includes */ +#include #include #include @@ -35,17 +36,12 @@ public: * Can be built incrementally so as to avoid costly integration at time of * factor construction. */ - class PreintegratedMeasurements { + class PreintegratedMeasurements : public PreintegratedRotation { friend class AHRSFactor; protected: Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer - Matrix3 measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3) - - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) public: @@ -61,31 +57,19 @@ public: PreintegratedMeasurements(const Vector3& bias, const Matrix3& measuredOmegaCovariance); + Vector3 biasHat() const { + return biasHat_; + } + const Matrix3& preintMeasCov() const { + return preintMeasCov_; + } + /// print void print(const std::string& s = "Preintegrated Measurements: ") const; /// equals bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; - const Matrix3& measurementCovariance() const { - return measurementCovariance_; - } - Matrix3 deltaRij() const { - return deltaRij_.matrix(); - } - double deltaTij() const { - return deltaTij_; - } - Vector3 biasHat() const { - return biasHat_; - } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; - } - const Matrix3& preintMeasCov() const { - return preintMeasCov_; - } - /// TODO: Document void resetIntegration(); @@ -103,12 +87,6 @@ public: Vector3 predict(const Vector3& bias, boost::optional H = boost::none) const; - /// Integrate coriolis correction in body frame rot_i - Vector3 integrateCoriolis(const Rot3& rot_i, - const Vector3& omegaCoriolis) const { - return rot_i.transpose() * omegaCoriolis * deltaTij_; - } - // This function is only used for test purposes // (compare numerical derivatives wrt analytic ones) static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, @@ -120,11 +98,8 @@ public: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); } }; @@ -132,7 +107,7 @@ private: typedef AHRSFactor This; typedef NoiseModelFactor3 Base; - PreintegratedMeasurements preintegratedMeasurements_; + PreintegratedMeasurements _PIM_; Vector3 gravity_; Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -178,7 +153,7 @@ public: /// Access the preintegrated measurements. const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; + return _PIM_; } const Vector3& omegaCoriolis() const { @@ -208,7 +183,7 @@ private: ar & boost::serialization::make_nvp("NoiseModelFactor3", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } 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 a2ce631ea..1c22bab9a 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -36,197 +36,136 @@ CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasu const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) : - biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - deltaRij_(Rot3()), deltaTij_(0.0), - delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) + PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration), + biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), + biasAccOmegaInit_(biasAccOmegaInit) { - measurementCovariance_.setZero(); - measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; - measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; - measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; - measurementCovariance_.block<3,3>(9,9) = biasAccCovariance; - measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance; - measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit; - PreintMeasCov_.setZero(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{ - cout << s << endl; - biasHat_.print(" biasHat"); - cout << " deltaTij " << deltaTij_ << endl; - cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; - deltaRij_.print(" deltaRij "); - cout << " measurementCovariance [ " << measurementCovariance_ << " ]" << endl; - cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl; + PreintegrationBase::print(s); + cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl; + cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl; + cout << " biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl; + cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{ - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol) + && equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol) + &&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol) + && equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) + && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; - delRdelBiasOmega_ = Z_3x3; - PreintMeasCov_.setZero(); + PreintegrationBase::resetIntegration(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, - double deltaT, boost::optional body_P_sensor) { + double deltaT, boost::optional body_P_sensor, + boost::optional F_test, boost::optional G_test) { // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate. // (i.e., we have to update jacobians and covariances before updating preintegrated measurements). - // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); + const Matrix3 R_i = deltaRij(); // store this + // Update preintegrated measurements. TODO Frank moved from end of this function !!! + Matrix9 F_9x9; + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9); // Single Jacobians to propagate covariance - Matrix3 H_pos_pos = I_3x3; - Matrix3 H_pos_vel = I_3x3 * deltaT; - Matrix3 H_pos_angles = Z_3x3; - - Matrix3 H_vel_pos = Z_3x3; - Matrix3 H_vel_vel = I_3x3; - Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT; - - Matrix3 H_angles_pos = Z_3x3; - Matrix3 H_angles_vel = Z_3x3; - Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + Matrix3 H_vel_biasacc = - R_i * deltaT; + Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix F(15,15); - F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3, - H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3, - H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega, - Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, - Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + // for documentation: + // F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, + // Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3, + // Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega, + // Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, + // Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3; + F.setZero(); + F.block<9,9>(0,0) = F_9x9; + F.block<6,6>(9,9) = I_6x6; + F.block<3,3>(3,9) = H_vel_biasacc; + F.block<3,3>(6,12) = H_angles_biasomega; // first order uncertainty propagation // Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose() - Matrix G_measCov_Gt = Matrix::Zero(15,15); - // BLOCK DIAGONAL TERMS - G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0); +// BLOCK DIAGONAL TERMS + G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance(); G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance_.block<3,3>(3,3) + measurementCovariance_.block<3,3>(15,15) ) * + (accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) * (H_vel_biasacc.transpose()); - G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance_.block<3,3>(6,6) + measurementCovariance_.block<3,3>(18,18) ) * + (gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) * (H_angles_biasomega.transpose()); - - G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9); - - G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12); - - // NEW OFF BLOCK DIAGONAL TERMS - Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) * H_angles_biasomega.transpose(); + G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_; + G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_; + // OFF BLOCK DIAGONAL TERMS + Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose(); G_measCov_Gt.block<3,3>(3,6) = block23; G_measCov_Gt.block<3,3>(6,3) = block23.transpose(); + preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt; - - // Update preintegrated measurements - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + // F_test and G_test are used for testing purposes and are not needed by the factor + if(F_test){ + F_test->resize(15,15); + (*F_test) << F; + } + if(G_test){ + G_test->resize(15,21); + // This is for testing & documentation + ///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) + (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, + Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, + Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, + Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3; } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; } //------------------------------------------------------------------------------ // CombinedImuFactor methods //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor() : - preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {} //------------------------------------------------------------------------------ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ -} + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), + _PIM_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const { @@ -243,255 +182,83 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << "," << keyFormatter(this->key6()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; + ImuFactorBase::print(""); + _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); } //------------------------------------------------------------------------------ bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ -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 { - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); + // error wrt bias evolution model (random walk) + Matrix6 Hbias_i, Hbias_j; + Vector6 fbias = traits::Between(bias_j, bias_i, + H6 ? &Hbias_j : 0, H5 ? &Hbias_i : 0).vector(); - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); + Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i; + Matrix93 D_r_vel_i, D_r_vel_j; - // We compute factor's Jacobians, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) + // 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); - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(15,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Z_3x3; - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Z_3x3, - //dBiasAcc/dPi - Z_3x3, Z_3x3, - //dBiasOmega/dPi - Z_3x3, Z_3x3; + // 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; } - if(H2) { - H2->resize(15,3); - (*H2) << - // dfP/dVi - - I_3x3 * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - I_3x3 - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Z_3x3, - //dBiasAcc/dVi - Z_3x3, - //dBiasOmega/dVi - Z_3x3; - } - - if(H3) { - H3->resize(15,6); - (*H3) << - // dfP/dPosej - Z_3x3, Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3, - //dBiasAcc/dPosej - Z_3x3, Z_3x3, - //dBiasOmega/dPosej - Z_3x3, Z_3x3; - } - - if(H4) { - H4->resize(15,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - I_3x3, - // dfR/dVj - Z_3x3, - //dBiasAcc/dVj - Z_3x3, - //dBiasOmega/dVj - Z_3x3; - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(15,6); - (*H5) << - // dfP/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias_i - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias_i - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega), - //dBiasAcc/dBias_i - -I_3x3, Z_3x3, - //dBiasOmega/dBias_i - Z_3x3, -I_3x3; - } - - if(H6) { - H6->resize(15,6); - (*H6) << - // dfP/dBias_j - Z_3x3, Z_3x3, - // dfV/dBias_j - Z_3x3, Z_3x3, - // dfR/dBias_j - Z_3x3, Z_3x3, - //dBiasAcc/dBias_j - I_3x3, Z_3x3, - //dBiasOmega/dBias_j - Z_3x3, I_3x3; - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer(); - - const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope(); - - Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15 - + // overall error + Vector r(15); + r << r_pvR, fbias; // vector of size 15 return r; } -//------------------------------------------------------------------------------ -PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){ - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - - return PoseVelocityBias(pose_j, vel_j, bias_i); -} - } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 5e2bfeadb..a7c6ecd39 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -23,7 +23,8 @@ /* GTSAM includes */ #include -#include +#include +#include #include namespace gtsam { @@ -33,78 +34,63 @@ namespace gtsam { * @addtogroup SLAM * * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally - * independent sets in factor graphs: a unifying perspective based on smart factors, - * Int. Conf. on Robotics and Automation (ICRA), 2014. + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating + * conditionally independent sets in factor graphs: a unifying perspective based + * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. * - * REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + ** REFERENCES: + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. */ /** - * Struct to hold all state variables of CombinedImuFactor returned by Predict function + * CombinedImuFactor is a 6-ways factor involving previous state (pose and + * velocity of the vehicle, as well as bias at previous time step), and current + * state (pose, velocity, bias at current time step). Following the pre- + * integration scheme proposed in [2], the CombinedImuFactor includes many IMU + * measurements, which are "summarized" using the CombinedPreintegratedMeasurements + * class. There are 3 main differences wrpt the ImuFactor class: + * 1) The factor is 6-ways, meaning that it also involves both biases (previous + * and current time step).Therefore, the factor internally imposes the biases + * to be slowly varying; in particular, the matrices "biasAccCovariance" and + * "biasOmegaCovariance" described the random walk that models bias evolution. + * 2) The preintegration covariance takes into account the noise in the bias + * estimate used for integration. + * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves + * the correlation between the bias uncertainty and the preintegrated + * measurements uncertainty. */ -struct PoseVelocityBias { - Pose3 pose; - Vector3 velocity; - imuBias::ConstantBias bias; - - PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, - const imuBias::ConstantBias _bias) : - pose(_pose), velocity(_velocity), bias(_bias) { - } -}; - -/** - * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias - * at previous time step), and current state (pose, velocity, bias at current time step). According to the - * preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are - * "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor: - * 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step). - * Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices - * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution. - * 2) The preintegration covariance takes into account the noise in the bias estimate used for integration. - * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty - * and the preintegrated measurements uncertainty. - */ -class CombinedImuFactor: public NoiseModelFactor6 { +class CombinedImuFactor: public NoiseModelFactor6, public ImuFactorBase{ public: - /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) - * and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor). - * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received - * from the IMU) so as to avoid costly integration at time of factor construction. + /** + * CombinedPreintegratedMeasurements integrates the IMU measurements + * (rotation rates and accelerations) and the corresponding covariance matrix. + * The measurements are then used to build the CombinedImuFactor. Integration + * is done incrementally (ideally, one integrates the measurement as soon as + * it is received from the IMU) so as to avoid costly integration at time of + * factor construction. */ - class CombinedPreintegratedMeasurements { + class CombinedPreintegratedMeasurements: public PreintegrationBase { friend class CombinedImuFactor; protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Eigen::Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector - ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21) - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j + Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk + Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk + Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - - Eigen::Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements + Eigen::Matrix preintMeasCov_; ///< Covariance matrix of the preintegrated measurements ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation ///< between the preintegrated measurements and the biases - bool use2ndOrderIntegration_; ///< Controls the order of integration - public: /** @@ -141,60 +127,20 @@ public: * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) */ void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); + boost::optional body_P_sensor = boost::none, + boost::optional F_test = boost::none, boost::optional G_test = boost::none); /// methods to access class variables - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix PreintMeasCov() const { return PreintMeasCov_;} - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ + Matrix preintMeasCov() const { return preintMeasCov_;} private: - /** Serialization function */ + + /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } }; @@ -203,12 +149,7 @@ private: typedef CombinedImuFactor This; typedef NoiseModelFactor6 Base; - CombinedPreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + CombinedPreintegratedMeasurements _PIM_; public: @@ -257,11 +198,7 @@ public: /** Access the preintegrated measurements. */ const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + return _PIM_; } /** implement functions needed to derive from Factor */ @@ -275,12 +212,6 @@ public: boost::optional H5 = boost::none, boost::optional H6 = boost::none) const; - /// predicted states from IMU - static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias_i, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); - private: /** Serialization function */ @@ -289,7 +220,7 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor6", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); + ar & BOOST_SERIALIZATION_NVP(_PIM_); ar & BOOST_SERIALIZATION_NVP(gravity_); ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); 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/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 9a9cc9d62..0aaa0122e 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -35,165 +35,82 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements( const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration) : - biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), - deltaRij_(Rot3()), deltaTij_(0.0), - delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), - delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), - delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration) + PreintegrationBase(bias, + measuredAccCovariance, measuredOmegaCovariance, + integrationErrorCovariance, use2ndOrderIntegration) { - measurementCovariance_.setZero(); - measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance; - measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance; - measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance; - PreintMeasCov_.setZero(9,9); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::print(const string& s) const { - cout << s << endl; - biasHat_.print(" biasHat"); - cout << " deltaTij " << deltaTij_ << endl; - cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << endl; - cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << endl; - deltaRij_.print(" deltaRij "); - cout << " measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl; - cout << " PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl; + PreintegrationBase::print(s); + cout << " preintMeasCov = \n [ " << preintMeasCov_ << " ]" << endl; } //------------------------------------------------------------------------------ bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const { - return biasHat_.equals(expected.biasHat_, tol) - && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol) - && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) - && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) - && deltaRij_.equals(expected.deltaRij_, tol) - && fabs(deltaTij_ - expected.deltaTij_) < tol - && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) - && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) - && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) - && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) - && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); + return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol) + && PreintegrationBase::equals(expected, tol); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::resetIntegration(){ - deltaPij_ = Vector3::Zero(); - deltaVij_ = Vector3::Zero(); - deltaRij_ = Rot3(); - deltaTij_ = 0.0; - delPdelBiasAcc_ = Z_3x3; - delPdelBiasOmega_ = Z_3x3; - delVdelBiasAcc_ = Z_3x3; - delVdelBiasOmega_ = Z_3x3; - delRdelBiasOmega_ = Z_3x3; - PreintMeasCov_.setZero(); + PreintegrationBase::resetIntegration(); + preintMeasCov_.setZero(); } //------------------------------------------------------------------------------ void ImuFactor::PreintegratedMeasurements::integrateMeasurement( const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor) { + boost::optional body_P_sensor, + OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { - // NOTE: order is important here because each update uses old values (i.e., we have to update - // jacobians and covariances before updating preintegrated measurements). + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - // First we compensate the measurements for the bias - Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc); - Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); - - // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame - if(body_P_sensor){ - Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); - // linear acceleration vector in the body frame - } - - const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement - const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement - - const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr + const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement + Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr + const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement // Update Jacobians - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; - }else{ - delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT; - delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix() - * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_; - } - delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT; - delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_; - delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT; + updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT); - // Update preintegrated measurements covariance + // Update preintegrated measurements (also get Jacobian) + const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test + Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) + updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F); + + // first order covariance propagation: // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework /* ----------------------------------------------------------------------------------------------------------------------- */ - const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3) - const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i); - - Rot3 Rot_j = deltaRij_ * Rincr; - const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3) - const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j); - - Matrix H_pos_pos = I_3x3; - Matrix H_pos_vel = I_3x3 * deltaT; - Matrix H_pos_angles = Z_3x3; - - Matrix H_vel_pos = Z_3x3; - Matrix H_vel_vel = I_3x3; - Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; - // analytic expression corresponding to the following numerical derivative - // Matrix H_vel_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); - - Matrix H_angles_pos = Z_3x3; - Matrix H_angles_vel = Z_3x3; - Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; - // analytic expression corresponding to the following numerical derivative - // Matrix H_angles_angles = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); - - // overall Jacobian wrt preintegrated measurements (df/dx) - Matrix F(9,9); - F << H_pos_pos, H_pos_vel, H_pos_angles, - H_vel_pos, H_vel_vel, H_vel_angles, - H_angles_pos, H_angles_vel, H_angles_angles; - - // first order uncertainty propagation: - // the deltaT allows to pass from continuous time noise to discrete time noise + // preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); + // NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT) - // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT - PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ; + // NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise, + // as G and measurementCovariance are blockdiagonal matrices + preintMeasCov_ = F * preintMeasCov_ * F.transpose(); + preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT; + preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT; + preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT; - // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT - // This in only kept for documentation. - // - // Matrix G(9,9); - // G << I_3x3 * deltaT, Z_3x3, Z_3x3, - // Z_3x3, deltaRij.matrix() * deltaT, Z_3x3, - // Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT; - // - // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose(); - - // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!) - /* ----------------------------------------------------------------------------------------------------------------------- */ - if(!use2ndOrderIntegration_){ - deltaPij_ += deltaVij_ * deltaT; - }else{ - deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT; + // F_test and G_test are given as output for testing purposes and are not needed by the factor + if(F_test){ // This in only for testing + (*F_test) << F; + } + if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise + // intNoise accNoise omegaNoise + (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos + Z_3x3, R_i * deltaT, Z_3x3, // vel + Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle } - deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT; - deltaRij_ = deltaRij_ * Rincr; - deltaTij_ += deltaT; } //------------------------------------------------------------------------------ // ImuFactor methods //------------------------------------------------------------------------------ ImuFactor::ImuFactor() : - preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){} + ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {} //------------------------------------------------------------------------------ ImuFactor::ImuFactor( @@ -202,13 +119,10 @@ ImuFactor::ImuFactor( const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor, const bool use2ndOrderCoriolis) : - Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias), - preintegratedMeasurements_(preintegratedMeasurements), - gravity_(gravity), - omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor), - use2ndOrderCoriolis_(use2ndOrderCoriolis){ -} + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), + pose_i, vel_i, pose_j, vel_j, bias), + ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis), + _PIM_(preintegratedMeasurements) {} //------------------------------------------------------------------------------ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const { @@ -224,215 +138,28 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const { << keyFormatter(this->key3()) << "," << keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ")\n"; - preintegratedMeasurements_.print(" preintegrated measurements:"); - cout << " gravity: [ " << gravity_.transpose() << " ]" << endl; - cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl; + ImuFactorBase::print(""); + _PIM_.print(" preintegrated measurements:"); this->noiseModel_->print(" noise model: "); - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); } //------------------------------------------------------------------------------ bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) - && equal_with_abs_tol(gravity_, e->gravity_, tol) - && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); + && _PIM_.equals(e->_PIM_, tol) + && ImuFactorBase::equals(*e, tol); } //------------------------------------------------------------------------------ -Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1, boost::optional H2, - boost::optional H3, boost::optional H4, - boost::optional H5) const -{ +Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, boost::optional H1, + boost::optional H2, boost::optional H3, + boost::optional H4, boost::optional H5) const { - const double& deltaTij = preintegratedMeasurements_.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope(); - - // we give some shorter name to rotations and translations - const Rot3 Rot_i = pose_i.rotation(); - const Rot3 Rot_j = pose_j.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - const Vector3 pos_j = pose_j.translation().vector(); - - // We compute factor's Jacobians - /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term - - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - - const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j)); - - const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected); - - const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij); - - const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - - if(H1) { - H1->resize(9,6); - - Matrix3 dfPdPi; - Matrix3 dfVdPi; - if(use2ndOrderCoriolis_){ - dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; - dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; - } - else{ - dfPdPi = - Rot_i.matrix(); - dfVdPi = Z_3x3; - } - - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), - // dfP/dPi - dfPdPi, - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), - // dfV/dPi - dfVdPi, - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Z_3x3; - } - - if(H2) { - H2->resize(9,3); - (*H2) << - // dfP/dVi - - I_3x3 * deltaTij - + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper - // dfV/dVi - - I_3x3 - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Z_3x3; - } - - if(H3) { - H3->resize(9,6); - (*H3) << - // dfP/dPosej - Z_3x3, Rot_j.matrix(), - // dfV/dPosej - Matrix::Zero(3,6), - // dfR/dPosej - Jrinv_fRhat * ( I_3x3 ), Z_3x3; - } - - if(H4) { - H4->resize(9,3); - (*H4) << - // dfP/dVj - Z_3x3, - // dfV/dVj - I_3x3, - // dfR/dVj - Z_3x3; - } - - if(H5) { - const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); - const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); - const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_; - - H5->resize(9,6); - (*H5) << - // dfP/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_, - // dfV/dBias - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_, - - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_, - // dfR/dBias - Matrix::Zero(3,3), - Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega); - } - - // Evaluate residual error, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 fp = - pos_j - pos_i - - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_ - + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr) - - vel_i * deltaTij - + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - - 0.5 * gravity_ * deltaTij*deltaTij; - - const Vector3 fv = - vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_ - + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr) - + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij // Coriolis term - - gravity_ * deltaTij; - - const Vector3 fR = Rot3::Logmap(fRhat); - - Vector r(9); r << fp, fv, fR; - return r; -} - -//------------------------------------------------------------------------------ -PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) -{ - - const double& deltaTij = preintegratedMeasurements.deltaTij_; - const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer(); - const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope(); - - const Rot3 Rot_i = pose_i.rotation(); - const Vector3 pos_i = pose_i.translation().vector(); - - // Predict state at time j - /* ---------------------------------------------------------------------------------------------------- */ - Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_ - + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; - - Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ - + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); - - if(use2ndOrderCoriolis){ - pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position - vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity - } - - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); - // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) - Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); - Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - - Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term - const Rot3 deltaRij_biascorrected_corioliscorrected = - Rot3::Expmap( theta_biascorrected_corioliscorrected ); - const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected ); - - Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); - return PoseVelocity(pose_j, vel_j); + return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5); } } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 2af634926..b91c76ade 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -23,7 +23,8 @@ /* GTSAM includes */ #include -#include +#include +#include #include namespace gtsam { @@ -38,66 +39,46 @@ namespace gtsam { * Int. Conf. on Robotics and Automation (ICRA), 2014. * ** REFERENCES: - * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built - * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012. - * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. + * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + * Volume 2, 2008. + * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + * High-Dynamic Motion in Built Environments Without Initial Conditions", + * TRO, 28(1):61-76, 2012. + * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + * Computation of the Jacobian Matrices", Tech. Report, 2013. */ /** - * Struct to hold return variables by the Predict Function + * ImuFactor is a 5-ways factor involving previous state (pose and velocity of + * the vehicle at previous time step), current state (pose and velocity at + * current time step), and the bias estimate. Following the preintegration + * scheme proposed in [2], the ImuFactor includes many IMU measurements, which + * are "summarized" using the PreintegratedMeasurements class. + * Note that this factor does not model "temporal consistency" of the biases + * (which are usually slowly varying quantities), which is up to the caller. + * See also CombinedImuFactor for a class that does this for you. */ -struct PoseVelocity { - Pose3 pose; - Vector3 velocity; - PoseVelocity(const Pose3& _pose, const Vector3& _velocity) : - pose(_pose), velocity(_velocity) { - } -}; - -/** - * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), - * current state (pose and velocity at current time step), and the bias estimate. According to the - * preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are - * "summarized" using the PreintegratedMeasurements class. - * Note that this factor does not force "temporal consistency" of the biases (which are usually - * slowly varying quantities), see also CombinedImuFactor for more details. - */ -class ImuFactor: public NoiseModelFactor5 { +class ImuFactor: public NoiseModelFactor5, public ImuFactorBase { public: /** * PreintegratedMeasurements accumulates (integrates) the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. - * The measurements are then used to build the Preintegrated IMU factor (ImuFactor). - * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received - * from the IMU) so as to avoid costly integration at time of factor construction. + * The measurements are then used to build the Preintegrated IMU factor. + * Integration is done incrementally (ideally, one integrates the measurement + * as soon as it is received from the IMU) so as to avoid costly integration + * at time of factor construction. */ - class PreintegratedMeasurements { + class PreintegratedMeasurements: public PreintegrationBase { friend class ImuFactor; protected: - imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration - Eigen::Matrix measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) - Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) - Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) - double deltaTij_; ///< Time interval from i to j - - Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias - Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias - Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias - Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias - Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - - Eigen::Matrix PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] + Eigen::Matrix preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION] ///< (first-order propagation from *measurementCovariance*). - bool use2ndOrderIntegration_; ///< Controls the order of integration - - public: + public: /** * Default constructor, initializes the class with no measurements @@ -127,160 +108,107 @@ public: * @param measuredOmega Measured angular velocity (as given by the sensor) * @param deltaT Time interval between two consecutive IMU measurements * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame) + * @param Fout, Gout Jacobians used internally (only needed for testing) */ void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, - boost::optional body_P_sensor = boost::none); + boost::optional body_P_sensor = boost::none, + OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none); /// methods to access class variables - Matrix measurementCovariance() const {return measurementCovariance_;} - Matrix deltaRij() const {return deltaRij_.matrix();} - double deltaTij() const{return deltaTij_;} - Vector deltaPij() const {return deltaPij_;} - Vector deltaVij() const {return deltaVij_;} - Vector biasHat() const { return biasHat_.vector();} - Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;} - Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;} - Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;} - Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;} - Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;} - Matrix preintMeasCov() const { return PreintMeasCov_;} - - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt, - const Vector3& delta_angles, const Vector& delta_vel_in_t0){ - // Note: all delta terms refer to an IMU\sensor system at t0 - Vector body_t_a_body = msr_acc_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt; - } - - // This function is only used for test purposes (compare numerical derivatives wrt analytic ones) - static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt, - const Vector3& delta_angles){ - // Note: all delta terms refer to an IMU\sensor system at t0 - // Calculate the corrected measurements using the Bias object - Vector body_t_omega_body= msr_gyro_t; - Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles); - R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt ); - return Rot3::Logmap(R_t_to_t0); - } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(biasHat_); - ar & BOOST_SERIALIZATION_NVP(measurementCovariance_); - ar & BOOST_SERIALIZATION_NVP(deltaPij_); - ar & BOOST_SERIALIZATION_NVP(deltaVij_); - ar & BOOST_SERIALIZATION_NVP(deltaRij_); - ar & BOOST_SERIALIZATION_NVP(deltaTij_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); - ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); - ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); - } - }; + Matrix preintMeasCov() const { return preintMeasCov_;} private: - typedef ImuFactor This; - typedef NoiseModelFactor5 Base; - - PreintegratedMeasurements preintegratedMeasurements_; - Vector3 gravity_; - Vector3 omegaCoriolis_; - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect - - public: - - /** Shorthand for a smart pointer to a factor */ -#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; -#else - typedef boost::shared_ptr shared_ptr; -#endif - - /** Default constructor - only use for serialization */ - ImuFactor(); - - /** - * Constructor - * @param pose_i Previous pose key - * @param vel_i Previous velocity key - * @param pose_j Current pose key - * @param vel_j Current velocity key - * @param bias Previous bias key - * @param preintegratedMeasurements Preintegrated IMU measurements - * @param gravity Gravity vector expressed in the global frame - * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame - * @param body_P_sensor Optional pose of the sensor frame in the body frame - * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect - */ - ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); - - virtual ~ImuFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const; - - /** implement functions needed for Testable */ - - /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - - /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; - - /** Access the preintegrated measurements. */ - - const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } - - const Vector3& gravity() const { return gravity_; } - - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } - - /** implement functions needed to derive from Factor */ - - /// vector of errors - Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none, - boost::optional H4 = boost::none, - boost::optional H5 = boost::none) const; - - /// predicted states from IMU - static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i, - const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false); - - private: - - /** Serialization function */ + /// Serialization function friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor5", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } - }; // class ImuFactor + }; - typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; + private: + + typedef ImuFactor This; + typedef NoiseModelFactor5 Base; + + PreintegratedMeasurements _PIM_; + + public: + + /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif + + /** Default constructor - only use for serialization */ + ImuFactor(); + + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias Previous bias key + * @param preintegratedMeasurements Preintegrated IMU measurements + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + const PreintegratedMeasurements& preintegratedMeasurements, + const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false); + + virtual ~ImuFactor() {} + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; + + /** implement functions needed for Testable */ + + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + + /** Access the preintegrated measurements. */ + + const PreintegratedMeasurements& preintegratedMeasurements() const { + return _PIM_; } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const; + + private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NoiseModelFactor5", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(_PIM_); + ar & BOOST_SERIALIZATION_NVP(gravity_); + ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; // class ImuFactor + +typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h new file mode 100644 index 000000000..1b7919a82 --- /dev/null +++ b/gtsam/navigation/ImuFactorBase.h @@ -0,0 +1,84 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +/* GTSAM includes */ +#include +#include + +namespace gtsam { + +class ImuFactorBase { + +protected: + + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + +public: + + /** Default constructor - only use for serialization */ + ImuFactorBase() : + gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), + body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + + /** + * Default constructor, stores basic quantities required by the Imu factors + * @param gravity Gravity vector expressed in the global frame + * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame + * @param body_P_sensor Optional pose of the sensor frame in the body frame + * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect + */ + ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : + gravity_(gravity), omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + + /// Methods to access class variables + const Vector3& gravity() const { return gravity_; } + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + + /// Needed for testable + //------------------------------------------------------------------------------ + void print(const std::string& s) const { + std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl; + std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl; + std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl; + if(this->body_P_sensor_) + this->body_P_sensor_->print(" sensor pose in body frame: "); + } + + /// Needed for testable + //------------------------------------------------------------------------------ + bool equals(const ImuFactorBase& expected, double tol) const { + return equal_with_abs_tol(gravity_, expected.gravity_, tol) + && equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol) + && (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_) + && ((!body_P_sensor_ && !expected.body_P_sensor_) || + (body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_))); + } + +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h new file mode 100644 index 000000000..67deb2d99 --- /dev/null +++ b/gtsam/navigation/PreintegratedRotation.h @@ -0,0 +1,141 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegratedRotation.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include + +namespace gtsam { + +/** + * PreintegratedRotation is the base class for all PreintegratedMeasurements + * classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). + * It includes the definitions of the preintegrated rotation. + */ +class PreintegratedRotation { + + Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i) + double deltaTij_; ///< Time interval from i to j + + /// Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delRdelBiasOmega_; + Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements + +public: + + /** + * Default constructor, initializes the variables in the base class + */ + PreintegratedRotation(const Matrix3& measuredOmegaCovariance) : + deltaRij_(Rot3()), deltaTij_(0.0), + delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {} + + /// methods to access class variables + Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive + Vector3 thetaRij(OptionalJacobian<3,3> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive + const double& deltaTij() const{return deltaTij_;} + const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;} + const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;} + + /// Needed for testable + void print(const std::string& s) const { + std::cout << s << std::endl; + std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl; + deltaRij_.print(" deltaRij "); + std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl; + std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl; + } + + /// Needed for testable + bool equals(const PreintegratedRotation& expected, double tol) const { + return deltaRij_.equals(expected.deltaRij_, tol) + && fabs(deltaTij_ - expected.deltaTij_) < tol + && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol) + && equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol); + } + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(){ + deltaRij_ = Rot3(); + deltaTij_ = 0.0; + delRdelBiasOmega_ = Z_3x3; + } + + /// Update preintegrated measurements + void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, + OptionalJacobian<3, 3> H = boost::none){ + deltaRij_ = deltaRij_.compose(incrR, H, boost::none); + deltaTij_ += deltaT; + } + + /** + * Update Jacobians to be used during preintegration + * TODO: explain arguments + */ + void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, + double deltaT) { + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT; + } + + /// Return a bias corrected version of the integrated rotation - expensive + Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const { + return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr); + } + + /// Get so<3> version of bias corrected rotation, with optional Jacobian + // Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) ) + Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, + OptionalJacobian<3, 3> H = boost::none) const { + // First, we correct deltaRij using the biasOmegaIncr, rotated + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + + if (H) { + Matrix3 Jrinv_theta_bc; + // This was done via an expmap, now we go *back* to so<3> + const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc); + const Matrix3 Jr_JbiasOmegaIncr = // + Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr); + (*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_; + return biascorrectedOmega; + } + //else + return Rot3::Logmap(deltaRij_biascorrected); + } + + /// Integrate coriolis correction in body frame rot_i + Vector3 integrateCoriolis(const Rot3& rot_i, + const Vector3& omegaCoriolis) const { + return rot_i.transpose() * omegaCoriolis * deltaTij(); + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(deltaRij_); + ar & BOOST_SERIALIZATION_NVP(deltaTij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + } +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h new file mode 100644 index 000000000..8214c1930 --- /dev/null +++ b/gtsam/navigation/PreintegrationBase.h @@ -0,0 +1,425 @@ +/* ---------------------------------------------------------------------------- + + * 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 PreintegrationBase.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Struct to hold all state variables of returned by Predict function + */ +struct PoseVelocityBias { + Pose3 pose; + Vector3 velocity; + imuBias::ConstantBias bias; + + PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, + const imuBias::ConstantBias _bias) : + pose(_pose), velocity(_velocity), bias(_bias) { + } +}; + +/** + * PreintegrationBase is the base class for PreintegratedMeasurements + * (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). + * It includes the definitions of the preintegrated variables and the methods + * to access, print, and compare them. + */ +class PreintegrationBase : public PreintegratedRotation { + + imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration + bool use2ndOrderIntegration_; ///< Controls the order of integration + + Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) + Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame) + + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + + Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements + Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty + /// (to compensate errors in Euler integration) + +public: + + /** + * Default constructor, initializes the variables in the base class + * @param bias Current estimate of acceleration and rotation rate biases + * @param use2ndOrderIntegration Controls the order of integration + * (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2) + */ + PreintegrationBase(const imuBias::ConstantBias& bias, + const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance, + const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) : + PreintegratedRotation(measuredOmegaCovariance), + biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration), + deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), + delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), + delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3), + accelerometerCovariance_(measuredAccCovariance), + integrationCovariance_(integrationErrorCovariance) {} + + /// methods to access class variables + const Vector3& deltaPij() const {return deltaPij_;} + const Vector3& deltaVij() const {return deltaVij_;} + const imuBias::ConstantBias& biasHat() const { return biasHat_;} + Vector6 biasHatVector() const { return biasHat_.vector();} // expensive + const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;} + const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;} + const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;} + const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;} + + const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;} + const Matrix3& integrationCovariance() const { return integrationCovariance_;} + + /// Needed for testable + void print(const std::string& s) const { + PreintegratedRotation::print(s); + std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl; + std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl; + std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl; + std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl; + biasHat_.print(" biasHat"); + } + + /// Needed for testable + bool equals(const PreintegrationBase& expected, double tol) const { + return PreintegratedRotation::equals(expected, tol) + && biasHat_.equals(expected.biasHat_, tol) + && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol) + && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol) + && equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol) + && equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol); + } + + /// Re-initialize PreintegratedMeasurements + void resetIntegration(){ + PreintegratedRotation::resetIntegration(); + deltaPij_ = Vector3::Zero(); + deltaVij_ = Vector3::Zero(); + delPdelBiasAcc_ = Z_3x3; + delPdelBiasOmega_ = Z_3x3; + delVdelBiasAcc_ = Z_3x3; + delVdelBiasOmega_ = Z_3x3; + } + + /// Update preintegrated measurements + void updatePreintegratedMeasurements(const Vector3& correctedAcc, + const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) { + + Matrix3 dRij = deltaRij(); // expensive + Vector3 temp = dRij * correctedAcc * deltaT; + if(!use2ndOrderIntegration_){ + deltaPij_ += deltaVij_ * deltaT; + }else{ + deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; + } + deltaVij_ += temp; + + Matrix3 R_i, F_angles_angles; + if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij + updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0); + + if(F){ + Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; + // pos vel angle + *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos + Z_3x3, I_3x3, F_vel_angles, // vel + Z_3x3, Z_3x3, F_angles_angles;// angle + } + } + + /// Update Jacobians to be used during preintegration + void updatePreintegratedJacobians(const Vector3& correctedAcc, + const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){ + Matrix3 dRij = deltaRij(); // expensive + Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega(); + if (!use2ndOrderIntegration_) { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT; + delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT; + } else { + delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT; + delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5); + } + delVdelBiasAcc_ += -dRij * deltaT; + delVdelBiasOmega_ += temp; + update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT); + } + + void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc, + const Vector3& measuredOmega, Vector3& correctedAcc, + Vector3& correctedOmega, boost::optional body_P_sensor) { + correctedAcc = biasHat_.correctAccelerometer(measuredAcc); + correctedOmega = biasHat_.correctGyroscope(measuredOmega); + + // Then compensate for sensor-body displacement: we express the quantities + // (originally in the IMU frame) into the body frame + if(body_P_sensor){ + Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); + correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame + Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); + correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); + // linear acceleration vector in the body frame + } + } + + /// Predict state at time j + //------------------------------------------------------------------------------ + PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false, + boost::optional deltaPij_biascorrected_out = boost::none, + boost::optional deltaVij_biascorrected_out = boost::none) const { + + const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + const Rot3& Rot_i = pose_i.rotation(); + const Vector3& pos_i = pose_i.translation().vector(); + + // Predict state at time j + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr; + if(deltaPij_biascorrected_out)// if desired, store this + *deltaPij_biascorrected_out = deltaPij_biascorrected; + + Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected + + vel_i * deltaTij() + - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij()*deltaTij(); + + Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr; + if(deltaVij_biascorrected_out)// if desired, store this + *deltaVij_biascorrected_out = deltaVij_biascorrected; + + Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected + - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term + + gravity * deltaTij()); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position + vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity + } + + const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); + // deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr) + + Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected); + Vector3 correctedOmega = biascorrectedOmega - + Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term + const Rot3 correctedDeltaRij = + Rot3::Expmap( correctedOmega ); + const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij ); + + Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) ); + return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant + } + + /// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j + //------------------------------------------------------------------------------ + Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const Vector3& gravity, + const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis, + OptionalJacobian<9, 6> H1 = boost::none, + OptionalJacobian<9, 3> H2 = boost::none, + OptionalJacobian<9, 6> H3 = boost::none, + OptionalJacobian<9, 3> H4 = boost::none, + OptionalJacobian<9, 6> H5 = boost::none) const { + + // We need the mismatch w.r.t. the biases used for preintegration + // const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary + const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope(); + + // we give some shorter name to rotations and translations + const Rot3& Ri = pose_i.rotation(); + const Rot3& Rj = pose_j.rotation(); + const Vector3& pos_j = pose_j.translation().vector(); + + // Evaluate residual error, according to [3] + /* ---------------------------------------------------------------------------------------------------- */ + Vector3 deltaPij_biascorrected, deltaVij_biascorrected; + PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, + omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() ); + + // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance + const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity ); + + // fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j) + + // Jacobian computation + /* ---------------------------------------------------------------------------------------------------- */ + // Get Get so<3> version of bias corrected rotation + // If H5 is asked for, we will need the Jacobian, which we store in H5 + // H5 will then be corrected below to take into account the Coriolis effect + Matrix3 D_cThetaRij_biasOmegaIncr; + Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0); + + // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration + const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); + const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); + Vector3 correctedOmega = biascorrectedOmega - coriolis; + + Rot3 correctedDeltaRij, fRrot; + Vector3 fR; + + // Accessory matrix, used to build the jacobians + Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot; + + // This is done to save computation: we ask for the jacobians only when they are needed + if(H1 || H2 || H3 || H4 || H5){ + correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRrot, D_fR_fRrot); + D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis); + }else{ + correctedDeltaRij = Rot3::Expmap( correctedOmega); + // Residual rotation error + fRrot = correctedDeltaRij.between(Ri.between(Rj)); + fR = Rot3::Logmap(fRrot); + } + if(H1) { + H1->resize(9,6); + Matrix3 dfPdPi = -I_3x3; + Matrix3 dfVdPi = Z_3x3; + if(use2ndOrderCoriolis){ + // this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() + Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose()); + dfPdPi += 0.5 * temp * deltaTij()*deltaTij(); + dfVdPi += temp * deltaTij(); + } + (*H1) << + // dfP/dRi + skewSymmetric(fp + deltaPij_biascorrected), + // dfP/dPi + dfPdPi, + // dfV/dRi + skewSymmetric(fv + deltaVij_biascorrected), + // dfV/dPi + dfVdPi, + // dfR/dRi + D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis), + // dfR/dPi + Z_3x3; + } + if(H2) { + H2->resize(9,3); + (*H2) << + // dfP/dVi + - Ri.transpose() * deltaTij() + + Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + // dfV/dVi + - Ri.transpose() + + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term + // dfR/dVi + Z_3x3; + } + if(H3) { + H3->resize(9,6); + (*H3) << + // dfP/dPosej + Z_3x3, Ri.transpose() * Rj.matrix(), + // dfV/dPosej + Matrix::Zero(3,6), + // dfR/dPosej + D_fR_fRrot * ( I_3x3 ), Z_3x3; + } + if(H4) { + H4->resize(9,3); + (*H4) << + // dfP/dVj + Z_3x3, + // dfV/dVj + Ri.transpose(), + // dfR/dVj + Z_3x3; + } + if(H5) { + // H5 by this point already contains 3*3 biascorrectedThetaRij derivative + const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr; + H5->resize(9,6); + (*H5) << + // dfP/dBias + - delPdelBiasAcc(), - delPdelBiasOmega(), + // dfV/dBias + - delVdelBiasAcc(), - delVdelBiasOmega(), + // dfR/dBias + Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega); + } + Vector9 r; r << fp, fv, fR; + return r; + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation); + ar & BOOST_SERIALIZATION_NVP(biasHat_); + ar & BOOST_SERIALIZATION_NVP(deltaPij_); + ar & BOOST_SERIALIZATION_NVP(deltaVij_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + } +}; + +class ImuBase { + +protected: + + Vector3 gravity_; + Vector3 omegaCoriolis_; + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + +public: + + ImuBase() : + gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)), + body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {} + + ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis, + boost::optional body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) : + gravity_(gravity), omegaCoriolis_(omegaCoriolis), + body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {} + + const Vector3& gravity() const { return gravity_; } + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index e6ecf42a3..25b3518bc 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -116,7 +116,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { /* ************************************************************************* */ TEST(AHRSFactor, Error) { // Linearization point - Vector3 bias; // Bias + Vector3 bias(0.,0.,0.); // Bias Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); @@ -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 @@ -241,7 +241,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) { Matrix expectedDelRdelBiasOmega = numericalDerivative11( boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign @@ -293,7 +293,7 @@ TEST( AHRSFactor, fistOrderExponential ) { Vector3 deltabiasOmega; deltabiasOmega << alpha, alpha, alpha; - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( + const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index aab38f258..d14eafb7d 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -39,19 +39,55 @@ using symbol_shorthand::X; using symbol_shorthand::V; using symbol_shorthand::B; -/* ************************************************************************* */ namespace { +/* ************************************************************************* */ +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedMeasurementsTest( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration) { -ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( + Matrix3 dRij = deltaRij_old.matrix(); + Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT; + Vector3 deltaPij_new; + if(!use2ndOrderIntegration){ + deltaPij_new = deltaPij_old + deltaVij_old * deltaT; + }else{ + deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; + } + Vector3 deltaVij_new = deltaVij_old + temp; + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT); + Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more + imuBias::ConstantBias bias_new(bias_old); + Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector(); + return result; +} + +Rot3 updatePreintegratedMeasurementsRot( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const imuBias::ConstantBias& bias_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration){ + + Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old, + bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration); + + return Rot3::Expmap(result.segment<3>(6)); +} + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ +CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) - ) -{ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity()); + const list& deltaTs){ + CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -59,7 +95,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) { result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT); } - return result; } @@ -67,20 +102,16 @@ Vector3 evaluatePreintegratedMeasurementsPosition( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return evaluatePreintegratedMeasurements(bias, - measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaPij(); + measuredAccs, measuredOmegas, deltaTs).deltaPij(); } Vector3 evaluatePreintegratedMeasurementsVelocity( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaVij(); } @@ -89,9 +120,7 @@ Rot3 evaluatePreintegratedMeasurementsRotation( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ) -{ + const list& deltaTs){ return Rot3(evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs).deltaRij()); } @@ -101,7 +130,6 @@ Rot3 evaluatePreintegratedMeasurementsRotation( /* ************************************************************************* */ TEST( CombinedImuFactor, PreintegratedMeasurements ) { - //cout << "++++++++++++++++++++++++++++++ PreintegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; // Linearization point imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases @@ -120,28 +148,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)); -// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases -// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc -// const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) -// const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) -// const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol)); -// EXPECT(assert_equal(Vector(expected1.deltaVij), Vector(actual1.deltaVij), tol)); -// EXPECT(assert_equal(expected1.deltaRij, actual1.deltaRij, tol)); -// DOUBLES_EQUAL(expected1.deltaTij, actual1.deltaTij, tol); + EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol)); + EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol)); + DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol); } - /* ************************************************************************* */ TEST( CombinedImuFactor, ErrorWithBiases ) { - //cout << "++++++++++++++++++++++++++++++ ErrorWithBiases +++++++++++++++++++++++++++++++++++++++ " << endl; - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); @@ -157,50 +174,37 @@ TEST( CombinedImuFactor, ErrorWithBiases ) double deltaT = 1.0; double tol = 1e-6; - // const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases - // const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc - // const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) - // const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - // const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements - Matrix I6x6(6,6); I6x6 = Matrix::Identity(6,6); - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( - imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), - Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); + CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data( + imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), + Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 ); - Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); - Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + // Expected Jacobians + Matrix H1e, H2e, H3e, H4e, H5e; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - EXPECT(assert_equal(errorExpected, errorActual.head(9), tol)); - - // Expected Jacobians - Matrix H1e, H2e, H3e, H4e, H5e; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e); - - - // Actual Jacobians + // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a, H6a; (void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a); @@ -214,7 +218,6 @@ TEST( CombinedImuFactor, ErrorWithBiases ) /* ************************************************************************* */ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) { - //cout << "++++++++++++++++++++++++++++++ FirstOrderPreIntegratedMeasurements +++++++++++++++++++++++++++++++++++++++ " << endl; // Linearization point imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases @@ -237,22 +240,22 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) } // Actual preintegrated values - ImuFactor::PreintegratedMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs); // Compute numerical derivatives Matrix expectedDelPdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelVdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelRdelBias = numericalDerivative11( - boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); + boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias); Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); @@ -265,6 +268,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } +/* ************************************************************************* */ TEST(CombinedImuFactor, PredictPositionAndVelocity){ imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) @@ -283,22 +287,21 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){ for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // Create factor - noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov()); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); - - // Predict - Pose3 x1; - Vector3 v1(0, 0.0, 0.0); - PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x1, v1, bias, Combined_pre_int_data, gravity, omegaCoriolis); - Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); - Vector3 expectedVelocity; expectedVelocity<<0,1,0; - EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); - EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); - + // Create factor + noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov()); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); + // Predict + Pose3 x1; + Vector3 v1(0, 0.0, 0.0); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); + Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); + Vector3 expectedVelocity; expectedVelocity<<0,1,0; + EXPECT(assert_equal(expectedPose, poseVelocityBias.pose)); + EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity))); } +/* ************************************************************************* */ TEST(CombinedImuFactor, PredictRotation) { imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) Matrix I6x6(6,6); @@ -319,14 +322,152 @@ TEST(CombinedImuFactor, PredictRotation) { // Predict Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)); Vector3 v(0,0,0); - PoseVelocityBias poseVelocityBias = Combinedfactor.Predict(x,v,bias, Combined_pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x,v,bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0)); EXPECT(assert_equal(expectedPose, poseVelocityBias.pose, tol)); } -#include +/* ************************************************************************* */ +TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) +{ + // Linearization point + imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + // Actual preintegrated values + CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, + body_P_sensor, Factual, Gactual); + + bool use2ndOrderIntegration = false; + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected F wrt positions (15,3) + Matrix df_dpos = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + _1, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + // rotation part has to be done properly, on manifold + df_dpos.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + _1, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + + // Compute expected F wrt velocities (15,3) + Matrix df_dvel = + numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, _1, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + // rotation part has to be done properly, on manifold + df_dvel.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, _1, deltaRij_old, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + + // Compute expected F wrt angles (15,3) + Matrix df_dangle = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, _1, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + // rotation part has to be done properly, on manifold + df_dangle.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, _1, bias_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + + // Compute expected F wrt biases (15,6) + Matrix df_dbias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + // rotation part has to be done properly, on manifold + df_dbias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + + Matrix Fexpected(15,15); + Fexpected << df_dpos, df_dvel, df_dangle, df_dbias; + EXPECT(assert_equal(Fexpected, Factual)); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected G wrt integration noise + Matrix df_dintNoise(15,3); + df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3; + + // Compute expected G wrt acc noise (15,3) + Matrix df_daccNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + // rotation part has to be done properly, on manifold + df_daccNoise.block<3,3>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + + // Compute expected G wrt gyro noise (15,3) + Matrix df_domegaNoise = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + // rotation part has to be done properly, on manifold + df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, bias_old, + newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + + // Compute expected G wrt bias random walk noise (15,6) + Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries + df_rwBias.setZero(); + df_rwBias.block<6,6>(9,0) = eye(6); + + // Compute expected G wrt gyro noise (15,6) + Matrix df_dinitBias = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsTest, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + // rotation part has to be done properly, on manifold + df_dinitBias.block<3,6>(6,0) = numericalDerivative11(boost::bind(&updatePreintegratedMeasurementsRot, + deltaPij_old, deltaVij_old, deltaRij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old); + df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows + + Matrix Gexpected(15,21); + Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias; + + EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + + (1/newDeltaT) * Gactual * Gactual.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); +} /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index cf81c32ae..0c4c9e3a6 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -37,6 +37,8 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { +// Auxiliary functions to test evaluate error in ImuFactor +/* ************************************************************************* */ Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias){ @@ -49,14 +51,48 @@ Rot3 evaluateRotationError(const ImuFactor& factor, return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; } +// Auxiliary functions to test Jacobians F and G used for +// covariance propagation during preintegration +/* ************************************************************************* */ +Vector updatePreintegratedPosVel( + const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old, + const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT, + const bool use2ndOrderIntegration_) { + + Matrix3 dRij = deltaRij_old.matrix(); + Vector3 temp = dRij * correctedAcc * deltaT; + Vector3 deltaPij_new; + if(!use2ndOrderIntegration_){ + deltaPij_new = deltaPij_old + deltaVij_old * deltaT; + }else{ + deltaPij_new += deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT; + } + Vector3 deltaVij_new = deltaVij_old + temp; + + Vector result(6); result << deltaPij_new, deltaVij_new; + return result; +} + +Rot3 updatePreintegratedRot(const Rot3& deltaRij_old, + const Vector3& correctedOmega, const double deltaT) { + Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT); + return deltaRij_new; +} + +// Auxiliary functions to test preintegrated Jacobians +// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ +/* ************************************************************************* */ +double accNoiseVar = 0.01; +double omegaNoiseVar = 0.03; +double intNoiseVar = 0.0001; ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( const imuBias::ConstantBias& bias, const list& measuredAccs, const list& measuredOmegas, const list& deltaTs, const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){ - ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(), - Matrix3::Identity(), Matrix3::Identity()); + ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(), + omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity()); list::const_iterator itAcc = measuredAccs.begin(); list::const_iterator itOmega = measuredOmegas.begin(); @@ -152,7 +188,7 @@ TEST( ImuFactor, PreintegratedMeasurements ) } /* ************************************************************************* */ -TEST( ImuFactor, Error ) +TEST( ImuFactor, ErrorAndJacobians ) { // Linearization point imuBias::ConstantBias bias; // Bias @@ -180,6 +216,77 @@ TEST( ImuFactor, Error ) Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); + + // Expected Jacobians + /////////////////// H1 /////////////////////////// + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + // Jacobians are around zero, so the rotation part is the same as: + Matrix H1Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + EXPECT(assert_equal(H1Rot3, H1e.bottomRows(3))); + EXPECT(assert_equal(H1e, H1a)); + + /////////////////// H2 /////////////////////////// + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + EXPECT(assert_equal(H2e, H2a)); + + /////////////////// H3 /////////////////////////// + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + // Jacobians are around zero, so the rotation part is the same as: + Matrix H3Rot3 = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + EXPECT(assert_equal(H3Rot3, H3e.bottomRows(3))); + EXPECT(assert_equal(H3e, H3a)); + + /////////////////// H4 /////////////////////////// + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + EXPECT(assert_equal(H4e, H4a)); + + /////////////////// H5 /////////////////////////// + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + EXPECT(assert_equal(H5e, H5a)); +} + +/* ************************************************************************* */ +TEST( ImuFactor, ErrorAndJacobianWithBiases ) +{ + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); + Vector3 v1(Vector3(0.5, 0.0, 0.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Vector3 v2(Vector3(0.5, 0.0, 0.0)); + + // Measurements + Vector3 gravity; gravity << 0, 0, 9.81; + Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; + Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3; + Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); + double deltaT = 1.0; + + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); + + // Expected error (should not be zero in this test, as we want to evaluate Jacobians + // at a nontrivial linearization point) + // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // Expected Jacobians Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); @@ -197,45 +304,27 @@ TEST( ImuFactor, Error ) boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); Matrix RH3e = numericalDerivative11( boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); // Actual Jacobians Matrix H1a, H2a, H3a, H4a, H5a; (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - // positions and velocities - Matrix H1etop6 = H1e.topRows(6); - Matrix H1atop6 = H1a.topRows(6); - EXPECT(assert_equal(H1etop6, H1atop6)); - // rotations - EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations - + EXPECT(assert_equal(H1e, H1a)); EXPECT(assert_equal(H2e, H2a)); - - // positions and velocities - Matrix H3etop6 = H3e.topRows(6); - Matrix H3atop6 = H3a.topRows(6); - EXPECT(assert_equal(H3etop6, H3atop6)); - // rotations - EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations - + EXPECT(assert_equal(H3e, H3a)); EXPECT(assert_equal(H4e, H4a)); - // EXPECT(assert_equal(H5e, H5a)); + EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ -TEST( ImuFactor, ErrorWithBiases ) +TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) { - // Linearization point -// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) -// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// Vector3 v1(Vector3(0.5, 0.0, 0.0)); -// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// Vector3 v2(Vector3(0.5, 0.0, 0.0)); - - imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) - Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); + imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot) + Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0)); Vector3 v1(Vector3(0.5, 0.0, 0.0)); - Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); Vector3 v2(Vector3(0.5, 0.0, 0.0)); // Measurements @@ -245,56 +334,57 @@ TEST( ImuFactor, ErrorWithBiases ) Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); - pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), + Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - // ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3)); - // pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + // Create factor + Pose3 bodyPsensor = Pose3(); + bool use2ndOrderCoriolis = true; + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + SETDEBUG("ImuFactor evaluateError", false); + Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); + SETDEBUG("ImuFactor evaluateError", false); - SETDEBUG("ImuFactor evaluateError", false); - Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias); - SETDEBUG("ImuFactor evaluateError", false); + // Expected error (should not be zero in this test, as we want to evaluate Jacobians + // at a nontrivial linearization point) + // Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; + // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - // Expected error - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; - // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // Expected Jacobians + Matrix H1e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); + Matrix H2e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); + Matrix H3e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); + Matrix H4e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); + Matrix H5e = numericalDerivative11( + boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); - Matrix H3e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); - Matrix H5e = numericalDerivative11( - boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); + // Check rotation Jacobians + Matrix RH1e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); + Matrix RH3e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); + Matrix RH5e = numericalDerivative11( + boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1); - Matrix RH3e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2); - Matrix RH5e = numericalDerivative11( - boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias); + // Actual Jacobians + Matrix H1a, H2a, H3a, H4a, H5a; + (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - // Actual Jacobians - Matrix H1a, H2a, H3a, H4a, H5a; - (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); - EXPECT(assert_equal(H4e, H4a)); - EXPECT(assert_equal(H5e, H5a)); + EXPECT(assert_equal(H1e, H1a)); + EXPECT(assert_equal(H2e, H2a)); + EXPECT(assert_equal(H3e, H3a)); + EXPECT(assert_equal(H4e, H4a)); + EXPECT(assert_equal(H5e, H5a)); } /* ************************************************************************* */ -TEST( ImuFactor, PartialDerivativeExpmap ) +TEST( ImuFactor, PartialDerivative_wrt_Bias ) { // Linearization point Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias @@ -307,7 +397,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -324,20 +414,14 @@ TEST( ImuFactor, PartialDerivativeLogmap ) // Measurements Vector3 deltatheta; deltatheta << 0, 0, 0; - // Compute numerical derivatives Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); - const Vector3 x = thetahat; // parametrization of so(3) - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - double normx = norm_2(x); - const Matrix3 actualDelFdeltheta = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; + Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); // Compare Jacobians EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); - } /* ************************************************************************* */ @@ -354,8 +438,7 @@ TEST( ImuFactor, fistOrderExponential ) double alpha = 0.0; Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha; - - const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); Matrix3 delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign @@ -366,7 +449,7 @@ TEST( ImuFactor, fistOrderExponential ) hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); - // Compare Jacobians + // This is a first order expansion so the equality is only an approximation EXPECT(assert_equal(expectedRot, actualRot)); } @@ -423,6 +506,128 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } +/* ************************************************************************* */ +TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) +{ + // Linearization point + imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases + Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1)); + + // Measurements + list measuredAccs, measuredOmegas; + list deltaTs; + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + measuredAccs.push_back(Vector3(0.1, 0.0, 0.0)); + measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0)); + deltaTs.push_back(0.01); + for(int i=1;i<100;i++) + { + measuredAccs.push_back(Vector3(0.05, 0.09, 0.01)); + measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0)); + deltaTs.push_back(0.01); + } + // Actual preintegrated values + ImuFactor::PreintegratedMeasurements preintegrated = + evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); + + // so far we only created a nontrivial linearization point for the preintegrated measurements + // Now we add a new measurement and ask for Jacobians + const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0); + const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0); + const double newDeltaT = 0.01; + const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement + const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement + const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement + + Matrix oldPreintCovariance = preintegrated.preintMeasCov(); + + Matrix Factual, Gactual; + preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, + body_P_sensor, Factual, Gactual); + + bool use2ndOrderIntegration = false; + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR F + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute expected f_pos_vel wrt positions + Matrix dfpv_dpos = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + _1, deltaVij_old, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old); + + // Compute expected f_pos_vel wrt velocities + Matrix dfpv_dvel = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, _1, deltaRij_old, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old); + + // Compute expected f_pos_vel wrt angles + Matrix dfpv_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, _1, + newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old); + + Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle; + + // Compute expected f_rot wrt angles + Matrix dfr_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedRot, + _1, newMeasuredOmega, newDeltaT), deltaRij_old); + + Matrix FexpectedBottom3(3,9); + FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle; + Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3; + + EXPECT(assert_equal(Fexpected, Factual)); + + ////////////////////////////////////////////////////////////////////////////////////////////// + // COMPUTE NUMERICAL DERIVATIVES FOR G + ////////////////////////////////////////////////////////////////////////////////////////////// + // Compute jacobian wrt integration noise + Matrix dgpv_dintNoise(6,3); + dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3; + + // Compute jacobian wrt acc noise + Matrix dgpv_daccNoise = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, deltaRij_old, + _1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc); + + // Compute expected F wrt gyro noise + Matrix dgpv_domegaNoise = + numericalDerivative11(boost::bind(&updatePreintegratedPosVel, + deltaPij_old, deltaVij_old, deltaRij_old, + newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega); + Matrix GexpectedTop6(6,9); + GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise; + + // Compute expected f_rot wrt gyro noise + Matrix dgr_dangle = + numericalDerivative11(boost::bind(&updatePreintegratedRot, + deltaRij_old, _1, newDeltaT), newMeasuredOmega); + + Matrix GexpectedBottom3(3,9); + GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle; + Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3; + + EXPECT(assert_equal(Gexpected, Gactual)); + + // Check covariance propagation + Matrix9 measurementCovariance; + measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3, + Z_3x3, accNoiseVar*I_3x3, Z_3x3, + Z_3x3, Z_3x3, omegaNoiseVar*I_3x3; + + Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() + + (1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose(); + + Matrix newPreintCovarianceActual = preintegrated.preintMeasCov(); + EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual)); +} + //#include ///* ************************************************************************* */ //TEST( ImuFactor, LinearizeTiming) @@ -561,13 +766,11 @@ TEST(ImuFactor, PredictPositionAndVelocity){ // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0)); Vector3 expectedVelocity; expectedVelocity<<0,1,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity))); - - } /* ************************************************************************* */ @@ -595,7 +798,7 @@ TEST(ImuFactor, PredictRotation) { // Predict Pose3 x1; Vector3 v1(0, 0.0, 0.0); - PoseVelocity poseVelocity = factor.Predict(x1, v1, bias, pre_int_data, gravity, omegaCoriolis); + PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis); Pose3 expectedPose(Rot3().ypr(M_PI/10, 0, 0), Point3(0, 0, 0)); Vector3 expectedVelocity; expectedVelocity<<0,0,0; EXPECT(assert_equal(expectedPose, poseVelocity.pose)); 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_unstable/nonlinear/CallRecord.h b/gtsam/nonlinear/CallRecord.h similarity index 100% rename from gtsam_unstable/nonlinear/CallRecord.h rename to gtsam/nonlinear/CallRecord.h diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 6c8561650..a86e7312a 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,11 +19,9 @@ #pragma once -#include +#include #include -#include -#include -#include +#include #include #include @@ -144,7 +142,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; @@ -309,57 +307,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 @@ -384,7 +336,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 @@ -460,15 +412,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; }; /** @@ -572,7 +524,8 @@ struct GenerateFunctionalNode: Argument, Base { /// Given df/dT, multiply in dT/dA and continue reverse AD process // Cols is always known at compile time template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, + JacobianMap& jacobians) const { Base::Record::reverseAD4(dFdT, jacobians); This::trace.reverseAD1(dFdT * This::dTdA, jacobians); } @@ -633,7 +586,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 6b68c7de2..3c9c4eeee 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -80,7 +80,7 @@ public: /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename UnaryExpression::OJ1) const) : + T (A::*method)(typename MakeOptionalJacobian::type) const) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -94,8 +94,8 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename BinaryExpression::OJ1, - typename BinaryExpression::OJ2) const, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), @@ -192,7 +192,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); @@ -256,7 +256,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_unstable/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h similarity index 92% rename from gtsam_unstable/nonlinear/ExpressionFactor.h rename to gtsam/nonlinear/ExpressionFactor.h index 94d79fbd4..4769e5048 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam/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/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..539302779 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_unstable/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h similarity index 97% rename from gtsam_unstable/nonlinear/expressionTesting.h rename to gtsam/nonlinear/expressionTesting.h index 4fd47eb76..ab6703f3a 100644 --- a/gtsam_unstable/nonlinear/expressionTesting.h +++ b/gtsam/nonlinear/expressionTesting.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include #include #include @@ -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/expressions.h b/gtsam/nonlinear/expressions.h similarity index 100% rename from gtsam_unstable/nonlinear/expressions.h rename to gtsam/nonlinear/expressions.h 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_unstable/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp similarity index 97% rename from gtsam_unstable/nonlinear/tests/testCallRecord.cpp rename to gtsam/nonlinear/tests/testCallRecord.cpp index 1cc674901..483b5ffa9 100644 --- a/gtsam_unstable/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -18,13 +18,12 @@ * @brief unit tests for CallRecord class */ -#include - -#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/nonlinear/tests/testExpressionFactor.cpp similarity index 99% rename from gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp rename to gtsam/nonlinear/tests/testExpressionFactor.cpp index 5758509eb..99b8120e3 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -17,12 +17,12 @@ * @brief unit tests for Block Automatic Differentiation */ -#include -#include -#include +#include #include #include #include +#include +#include #include #include @@ -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/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 941728d8c..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); @@ -470,6 +475,15 @@ TEST(Values, Destructors) { LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount); } +/* ************************************************************************* */ +TEST(Values, FixedSize) { + Values values; + Vector v(3); v << 5.0, 6.0, 7.0; + values.insertFixed(key1, v, 3); + Vector3 expected(5.0, 6.0, 7.0); + CHECK(assert_equal((Vector)expected, values.at(key1))); + CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ 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/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index ecf8adfec..243331dc1 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -453,6 +453,12 @@ public: } } + /// Gradient wrt a key at any values + Vector gradient(Key key, const VectorValues& x) const { + throw std::runtime_error("gradient for RegularImplicitSchurFactor is not implemented yet"); + } + + }; // RegularImplicitSchurFactor 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_unstable/slam/expressions.h b/gtsam/slam/expressions.h similarity index 96% rename from gtsam_unstable/slam/expressions.h rename to gtsam/slam/expressions.h index 7badc9dd7..1d9ddd6d4 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -7,7 +7,7 @@ #pragma once -#include +#include #include #include 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/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index cd42b8d4f..6abfe4336 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -3,6 +3,7 @@ set (gtsam_unstable_subdirs base geometry + linear discrete dynamics nonlinear @@ -47,6 +48,7 @@ endforeach(subdir) set(gtsam_unstable_srcs ${base_srcs} ${geometry_srcs} + ${linear_srcs} ${discrete_srcs} ${dynamics_srcs} ${nonlinear_srcs} 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 b1c254a9e..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::dExpInv_exp(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 bdc71fea2..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::dExpInv_exp(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/CMakeLists.txt b/gtsam_unstable/linear/CMakeLists.txt new file mode 100644 index 000000000..99a4b814e --- /dev/null +++ b/gtsam_unstable/linear/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB linear_headers "*.h") +install(FILES ${linear_headers} DESTINATION include/gtsam_unstable/linear) + +# Add all tests +add_subdirectory(tests) diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h new file mode 100644 index 000000000..bc1b2bc12 --- /dev/null +++ b/gtsam_unstable/linear/LinearEquality.h @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * LinearEquality.h + * @brief: LinearEquality derived from Base with constrained noise model + * @date: Nov 27, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * This class defines Linear constraints by inherit Base + * with the special Constrained noise model + */ +class LinearEquality: public JacobianFactor { +public: + typedef LinearEquality This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +private: + Key dualKey_; + +public: + /** default constructor for I/O */ + LinearEquality() : + Base() { + } + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit LinearEquality(const HessianFactor& hf) { + throw std::runtime_error("Cannot convert HessianFactor to LinearEquality"); + } + + /** Construct unary factor */ + LinearEquality(Key i1, const Matrix& A1, const Vector& b, Key dualKey) : + Base(i1, A1, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Construct binary factor */ + LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, + const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, b, noiseModel::Constrained::All(b.rows())), dualKey_( + dualKey) { + } + + /** Construct ternary factor */ + LinearEquality(Key i1, const Matrix& A1, Key i2, const Matrix& A2, Key i3, + const Matrix& A3, const Vector& b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, b, noiseModel::Constrained::All(b.rows())), dualKey_( + dualKey) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearEquality(const TERMS& terms, const Vector& b, Key dualKey) : + Base(terms, b, noiseModel::Constrained::All(b.rows())), dualKey_(dualKey) { + } + + /** Virtual destructor */ + virtual ~LinearEquality() { + } + + /** equals */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + Base::print(s, formatter); + } + + /** Clone this LinearEquality */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + /// dual key + Key dualKey() const { return dualKey_; } + + /// for active set method: equality constraints are always active + bool active() const { return true; } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for constraints. + * I think it should be zero, as this function is meant for objective cost. + * But the name "error" can be misleading. + * TODO: confirm with Frank!! */ + virtual double error(const VectorValues& c) const { + return 0.0; + } + +}; // \ LinearEquality + + +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/LinearEqualityFactorGraph.h b/gtsam_unstable/linear/LinearEqualityFactorGraph.h new file mode 100644 index 000000000..9c067ae3d --- /dev/null +++ b/gtsam_unstable/linear/LinearEqualityFactorGraph.h @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * LinearEqualityFactorGraph.h + * @brief: Factor graph of all LinearEquality factors + * @date: Dec 8, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class LinearEqualityFactorGraph : public FactorGraph { +public: + typedef boost::shared_ptr shared_ptr; +}; + +/// traits +template<> struct traits : public Testable< + LinearEqualityFactorGraph> { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h new file mode 100644 index 000000000..7c62c3d54 --- /dev/null +++ b/gtsam_unstable/linear/LinearInequality.h @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * LinearInequality.h + * @brief: LinearInequality derived from Base with constrained noise model + * @date: Nov 27, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include + +namespace gtsam { + +typedef Eigen::RowVectorXd RowVector; + +/** + * This class defines Linear constraints by inherit Base + * with the special Constrained noise model + */ +class LinearInequality: public JacobianFactor { +public: + typedef LinearInequality This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +private: + Key dualKey_; + bool active_; + +public: + /** default constructor for I/O */ + LinearInequality() : + Base(), active_(true) { + } + + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + explicit LinearInequality(const HessianFactor& hf) { + throw std::runtime_error( + "Cannot convert HessianFactor to LinearInequality"); + } + + /** Construct unary factor */ + LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) : + Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( + dualKey), active_(true) { + } + + /** Construct binary factor */ + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b, + Key dualKey) : + Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( + dualKey), active_(true) { + } + + /** Construct ternary factor */ + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, + const RowVector& A3, double b, Key dualKey) : + Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(), + noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearInequality(const TERMS& terms, double b, Key dualKey) : + Base(terms, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( + dualKey), active_(true) { + } + + /** Virtual destructor */ + virtual ~LinearInequality() { + } + + /** equals */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + if (active()) + Base::print(s + " Active", formatter); + else + Base::print(s + " Inactive", formatter); + } + + /** Clone this LinearInequality */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + boost::make_shared(*this)); + } + + /// dual key + Key dualKey() const { return dualKey_; } + + /// return true if this constraint is active + bool active() const { return active_; } + + /// Make this inequality constraint active + void activate() { active_ = true; } + + /// Make this inequality constraint inactive + void inactivate() { active_ = false; } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for single-valued inequality constraints. */ + virtual double error(const VectorValues& c) const { + return error_vector(c)[0]; + } + + /** dot product of row s with the corresponding vector in p */ + double dotProductRow(const VectorValues& p) const { + double aTp = 0.0; + for (const_iterator xj = begin(); xj != end(); ++xj) { + Vector pj = p.at(*xj); + Vector aj = getA(xj).transpose(); + aTp += aj.dot(pj); + } + return aTp; + } + +}; // \ LinearInequality + +/// traits +template<> struct traits : public Testable {}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h new file mode 100644 index 000000000..eca271941 --- /dev/null +++ b/gtsam_unstable/linear/LinearInequalityFactorGraph.h @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * LinearInequalityFactorGraph.h + * @brief: Factor graph of all LinearInequality factors + * @date: Dec 8, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class LinearInequalityFactorGraph: public FactorGraph { +private: + typedef FactorGraph Base; + +public: + typedef boost::shared_ptr shared_ptr; + + /** print */ + void print(const std::string& str, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + Base::print(str, keyFormatter); + } + + /** equals */ + bool equals(const LinearInequalityFactorGraph& other, + double tol = 1e-9) const { + return Base::equals(other, tol); + } +}; + +/// traits +template<> struct traits : public Testable< + LinearInequalityFactorGraph> { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h new file mode 100644 index 000000000..111ab506f --- /dev/null +++ b/gtsam_unstable/linear/QP.h @@ -0,0 +1,58 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * QP.h + * @brief: Factor graphs of a Quadratic Programming problem + * @date: Dec 8, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * struct contains factor graphs of a Quadratic Programming problem + */ +struct QP { + GaussianFactorGraph cost; //!< Quadratic cost factors + LinearEqualityFactorGraph equalities; //!< linear equality constraints + LinearInequalityFactorGraph inequalities; //!< linear inequality constraints + + /** default constructor */ + QP() : + cost(), equalities(), inequalities() { + } + + /** constructor */ + QP(const GaussianFactorGraph& _cost, + const LinearEqualityFactorGraph& _linearEqualities, + const LinearInequalityFactorGraph& _linearInequalities) : + cost(_cost), equalities(_linearEqualities), inequalities( + _linearInequalities) { + } + + /** print */ + void print(const std::string& s = "") { + std::cout << s << std::endl; + cost.print("Quadratic cost factors: "); + equalities.print("Linear equality factors: "); + inequalities.print("Linear inequality factors: "); + } +}; + +} // namespace gtsam + diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp new file mode 100644 index 000000000..f0eb7d7fb --- /dev/null +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -0,0 +1,252 @@ +/* + * QPSolver.cpp + * @brief: + * @date: Apr 15, 2014 + * @author: thduynguyen + */ + +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { + +//****************************************************************************** +QPSolver::QPSolver(const QP& qp) : qp_(qp) { + baseGraph_ = qp_.cost; + baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end()); + costVariableIndex_ = VariableIndex(qp_.cost); + equalityVariableIndex_ = VariableIndex(qp_.equalities); + inequalityVariableIndex_ = VariableIndex(qp_.inequalities); + constrainedKeys_ = qp_.equalities.keys(); + constrainedKeys_.merge(qp_.inequalities.keys()); +} + +//****************************************************************************** +VectorValues QPSolver::solveWithCurrentWorkingSet( + const LinearInequalityFactorGraph& workingSet) const { + GaussianFactorGraph workingGraph = baseGraph_; + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { + if (factor->active()) + workingGraph.push_back(factor); + } + return workingGraph.optimize(); +} + +//****************************************************************************** +JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, + const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { + + // Transpose the A matrix of constrained factors to have the jacobian of the dual key + std::vector > Aterms = collectDualJacobians + < LinearEquality > (key, qp_.equalities, equalityVariableIndex_); + std::vector > AtermsInequalities = collectDualJacobians + < LinearInequality > (key, workingSet, inequalityVariableIndex_); + Aterms.insert(Aterms.end(), AtermsInequalities.begin(), + AtermsInequalities.end()); + + // Collect the gradients of unconstrained cost factors to the b vector + if (Aterms.size() > 0) { + Vector b = zero(delta.at(key).size()); + if (costVariableIndex_.find(key) != costVariableIndex_.end()) { + BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { + GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); + b += factor->gradient(key, delta); + } + } + return boost::make_shared(Aterms, b, noiseModel::Constrained::All(b.rows())); + } + else { + return boost::make_shared(); + } +} + +//****************************************************************************** +GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( + const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { + GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); + BOOST_FOREACH(Key key, constrainedKeys_) { + // Each constrained key becomes a factor in the dual graph + JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta); + if (!dualFactor->empty()) + dualGraph->push_back(dualFactor); + } + return dualGraph; +} + +//****************************************************************************** +int QPSolver::identifyLeavingConstraint( + const LinearInequalityFactorGraph& workingSet, + const VectorValues& lambdas) const { + int worstFactorIx = -1; + // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either + // inactive or a good inequality constraint, so we don't care! + double maxLambda = 0.0; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + if (factor->active()) { + double lambda = lambdas.at(factor->dualKey())[0]; + if (lambda > maxLambda) { + worstFactorIx = factorIx; + maxLambda = lambda; + } + } + } + return worstFactorIx; +} + +//****************************************************************************** +/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints + * If some inactive inequality constraints complain about the full step (alpha = 1), + * we have to adjust alpha to stay within the inequality constraints' feasible regions. + * + * For each inactive inequality j: + * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints + * - We want: aj'*(xk + alpha*p) - bj <= 0 + * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0 + * it's good! + * - We only care when aj'*p > 0. In this case, we need to choose alpha so that + * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p) + * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p) + * + * We want the minimum of all those alphas among all inactive inequality. + */ +boost::tuple QPSolver::computeStepSize( + const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p) const { + static bool debug = false; + + double minAlpha = 1.0; + int closestFactorIx = -1; + for(size_t factorIx = 0; factorIxgetb()[0]; + // only check inactive factors + if (!factor->active()) { + // Compute a'*p + double aTp = factor->dotProductRow(p); + + // Check if a'*p >0. Don't care if it's not. + if (aTp <= 0) + continue; + + // Compute a'*xk + double aTx = factor->dotProductRow(xk); + + // alpha = (b - a'*xk) / (a'*p) + double alpha = (b - aTx) / aTp; + if (debug) + cout << "alpha: " << alpha << endl; + + // We want the minimum of all those max alphas + if (alpha < minAlpha) { + closestFactorIx = factorIx; + minAlpha = alpha; + } + } + + } + + return boost::make_tuple(minAlpha, closestFactorIx); +} + +//****************************************************************************** +QPState QPSolver::iterate(const QPState& state) const { + static bool debug = false; + + // Solve with the current working set + VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); + if (debug) + newValues.print("New solution:"); + + // If we CAN'T move further + if (newValues.equals(state.values, 1e-5)) { + // Compute lambda from the dual graph + if (debug) + cout << "Building dual graph..." << endl; + GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); + if (debug) + dualGraph->print("Dual graph: "); + VectorValues duals = dualGraph->optimize(); + if (debug) + duals.print("Duals :"); + + int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); + if (debug) + cout << "leavingFactor: " << leavingFactor << endl; + + // If all inequality constraints are satisfied: We have the solution!! + if (leavingFactor < 0) { + return QPState(newValues, duals, state.workingSet, true); + } + else { + // Inactivate the leaving constraint + LinearInequalityFactorGraph newWorkingSet = state.workingSet; + newWorkingSet.at(leavingFactor)->inactivate(); + return QPState(newValues, duals, newWorkingSet, false); + } + } + else { + // If we CAN make some progress + // Adapt stepsize if some inactive constraints complain about this move + double alpha; + int factorIx; + VectorValues p = newValues - state.values; + boost::tie(alpha, factorIx) = // + computeStepSize(state.workingSet, state.values, p); + if (debug) + cout << "alpha, factorIx: " << alpha << " " << factorIx << " " + << endl; + + // also add to the working set the one that complains the most + LinearInequalityFactorGraph newWorkingSet = state.workingSet; + if (factorIx >= 0) + newWorkingSet.at(factorIx)->activate(); + + // step! + newValues = state.values + alpha * p; + + return QPState(newValues, state.duals, newWorkingSet, false); + } +} + +//****************************************************************************** +LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( + const LinearInequalityFactorGraph& inequalities, + const VectorValues& initialValues) const { + LinearInequalityFactorGraph workingSet; + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){ + LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); + double error = workingFactor->error(initialValues); + if (fabs(error)>1e-7){ + workingFactor->inactivate(); + } else { + workingFactor->activate(); + } + workingSet.push_back(workingFactor); + } + return workingSet; +} + +//****************************************************************************** +pair QPSolver::optimize( + const VectorValues& initialValues) const { + + // Initialize workingSet from the feasible initialValues + LinearInequalityFactorGraph workingSet = + identifyActiveConstraints(qp_.inequalities, initialValues); + QPState state(initialValues, VectorValues(), workingSet, false); + + /// main loop of the solver + while (!state.converged) { + state = iterate(state); + } + + return make_pair(state.values, state.duals); +} + +} /* namespace gtsam */ diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h new file mode 100644 index 000000000..f7a575f8c --- /dev/null +++ b/gtsam_unstable/linear/QPSolver.h @@ -0,0 +1,188 @@ +/* + * QPSolver.h + * @brief: A quadratic programming solver implements the active set method + * @date: Apr 15, 2014 + * @author: thduynguyen + */ + +#pragma once + +#include +#include + +#include +#include + +namespace gtsam { + +/// This struct holds the state of QPSolver at each iteration +struct QPState { + VectorValues values; + VectorValues duals; + LinearInequalityFactorGraph workingSet; + bool converged; + + /// default constructor + QPState() : + values(), duals(), workingSet(), converged(false) { + } + + /// constructor with initial values + QPState(const VectorValues& initialValues, const VectorValues& initialDuals, + const LinearInequalityFactorGraph& initialWorkingSet, bool _converged) : + values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged( + _converged) { + } +}; + +/** + * This class implements the active set method to solve quadratic programming problems + * encoded in a GaussianFactorGraph with special mixed constrained noise models, in which + * a negative sigma denotes an inequality <=0 constraint, + * a zero sigma denotes an equality =0 constraint, + * and a positive sigma denotes a normal Gaussian noise model. + */ +class QPSolver { + + const QP& qp_; //!< factor graphs of the QP problem, can't be modified! + GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process. + VariableIndex costVariableIndex_, equalityVariableIndex_, + inequalityVariableIndex_; + FastSet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph + +public: + /// Constructor + QPSolver(const QP& qp); + + /// Find solution with the current working set + VectorValues solveWithCurrentWorkingSet( + const LinearInequalityFactorGraph& workingSet) const; + + /// @name Build the dual graph + /// @{ + + /// Collect the Jacobian terms for a dual factor + template + std::vector > collectDualJacobians(Key key, + const FactorGraph& graph, + const VariableIndex& variableIndex) const { + std::vector > Aterms; + if (variableIndex.find(key) != variableIndex.end()) { + BOOST_FOREACH(size_t factorIx, variableIndex[key]){ + typename FACTOR::shared_ptr factor = graph.at(factorIx); + if (!factor->active()) continue; + Matrix Ai = factor->getA(factor->find(key)).transpose(); + Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + } + } + return Aterms; + } + + /// Create a dual factor + JacobianFactor::shared_ptr createDualFactor(Key key, + const LinearInequalityFactorGraph& workingSet, + const VectorValues& delta) const; + + /** + * Build the dual graph to solve for the Lagrange multipliers. + * + * The Lagrangian function is: + * L(X,lambdas) = f(X) - \sum_k lambda_k * c_k(X), + * where the unconstrained part is + * f(X) = 0.5*X'*G*X - X'*g + 0.5*f0 + * and the linear equality constraints are + * c1(X), c2(X), ..., cm(X) + * + * Take the derivative of L wrt X at the solution and set it to 0, we have + * \grad f(X) = \sum_k lambda_k * \grad c_k(X) (*) + * + * For each set of rows of (*) corresponding to a variable xi involving in some constraints + * we have: + * \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi + * \grad c_k(xi) = \frac{\partial c_k}{\partial xi}' + * + * Note: If xi does not involve in any constraint, we have the trivial condition + * \grad f(Xi) = 0, which should be satisfied as a usual condition for unconstrained variables. + * + * So each variable xi involving in some constraints becomes a linear factor A*lambdas - b = 0 + * on the constraints' lambda multipliers, as follows: + * - The jacobian term A_k for each lambda_k is \grad c_k(xi) + * - The constant term b is \grad f(xi), which can be computed from all unconstrained + * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi + */ + GaussianFactorGraph::shared_ptr buildDualGraph( + const LinearInequalityFactorGraph& workingSet, + const VectorValues& delta) const; + + /// @} + + /** + * The goal of this function is to find currently active inequality constraints + * that violate the condition to be active. The one that violates the condition + * the most will be removed from the active set. See Nocedal06book, pg 469-471 + * + * Find the BAD active inequality that pulls x strongest to the wrong direction + * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0) + * + * For active inequality constraints (those that are enforced as equality constraints + * in the current working set), we want lambda < 0. + * This is because: + * - From the Lagrangian L = f - lambda*c, we know that the constraint force + * is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay + * on the constraint surface, the constraint force has to balance out with + * other unconstrained forces that are pulling x towards the unconstrained + * minimum point. The other unconstrained forces are pulling x toward (-\grad f), + * hence the constraint force has to be exactly \grad f, so that the total + * force is 0. + * - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0), + * while we are solving for - (<=0) constraint. + * - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction + * i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied. + * That means we want lambda < 0. + * - This is because when the constrained force pulls x towards the infeasible region (+), + * the unconstrained force is pulling x towards the opposite direction into + * the feasible region (again because the total force has to be 0 to make x stay still) + * So we can drop this constraint to have a lower error but feasible solution. + * + * In short, active inequality constraints with lambda > 0 are BAD, because they + * violate the condition to be active. + * + * And we want to remove the worst one with the largest lambda from the active set. + * + */ + int identifyLeavingConstraint(const LinearInequalityFactorGraph& workingSet, + const VectorValues& lambdas) const; + + /** + * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] + * + * @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex) + * is the constraint that has minimum alpha, or (-1,-1) if alpha = 1. + * This constraint will be added to the working set and become active + * in the next iteration + */ + boost::tuple computeStepSize( + const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p) const; + + /** Iterate 1 step, return a new state with a new workingSet and values */ + QPState iterate(const QPState& state) const; + + /** + * Identify active constraints based on initial values. + */ + LinearInequalityFactorGraph identifyActiveConstraints( + const LinearInequalityFactorGraph& inequalities, + const VectorValues& initialValues) const; + + /** Optimize with a provided initial values + * For this version, it is the responsibility of the caller to provide + * a feasible initial value. + * @return a pair of solutions + */ + std::pair optimize( + const VectorValues& initialValues) const; + +}; + +} /* namespace gtsam */ diff --git a/gtsam_unstable/linear/tests/CMakeLists.txt b/gtsam_unstable/linear/tests/CMakeLists.txt new file mode 100644 index 000000000..43df23daa --- /dev/null +++ b/gtsam_unstable/linear/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(linear_unstable "test*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp new file mode 100644 index 000000000..bf41743a2 --- /dev/null +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -0,0 +1,237 @@ +/* ---------------------------------------------------------------------------- + + * 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 testLinearEquality.cpp + * @brief Unit tests for LinearEquality + * @author thduynguyen + **/ + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +GTSAM_CONCEPT_TESTABLE_INST(LinearEquality) + +namespace { + namespace simple { + // Terms we'll use + const vector > terms = list_of > + (make_pair(5, Matrix3::Identity())) + (make_pair(10, 2*Matrix3::Identity())) + (make_pair(15, 3*Matrix3::Identity())); + + // RHS and sigmas + const Vector b = (Vector(3) << 1., 2., 3.).finished(); + const SharedDiagonal noise = noiseModel::Constrained::All(3); + } +} + +/* ************************************************************************* */ +TEST(LinearEquality, constructors_and_accessors) +{ + using namespace simple; + + // Test for using different numbers of terms + { + // One term constructor + LinearEquality expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); + LinearEquality actual(terms[0].first, terms[0].second, b, 0); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } + { + // Two term constructor + LinearEquality expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); + LinearEquality actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, b, 0); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } + { + // Three term constructor + LinearEquality expected( + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); + LinearEquality actual(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); + EXPECT(assert_equal(expected, actual)); + LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); + EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); + EXPECT(assert_equal(b, expected.getb())); + EXPECT(assert_equal(b, actual.getb())); + EXPECT(assert_equal(*noise, *actual.get_model())); + } +} + +/* ************************************************************************* */ +TEST(LinearEquality, Hessian_conversion) { + HessianFactor hessian(0, (Matrix(4,4) << + 1.57, 2.695, -1.1, -2.35, + 2.695, 11.3125, -0.65, -10.225, + -1.1, -0.65, 1, 0.5, + -2.35, -10.225, 0.5, 9.25).finished(), + (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), + 73.1725); + + try { + LinearEquality actual(hessian); + EXPECT(false); + } + catch (const std::runtime_error& exception) { + EXPECT(true); + } +} + +/* ************************************************************************* */ +TEST(LinearEquality, error) +{ + LinearEquality factor(simple::terms, simple::b, 0); + + VectorValues values; + values.insert(5, Vector::Constant(3, 1.0)); + values.insert(10, Vector::Constant(3, 0.5)); + values.insert(15, Vector::Constant(3, 1.0/3.0)); + + Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0; + Vector actual_unwhitened = factor.unweighted_error(values); + EXPECT(assert_equal(expected_unwhitened, actual_unwhitened)); + + // whitened is meaningless in constraints + Vector expected_whitened(3); expected_whitened = expected_unwhitened; + Vector actual_whitened = factor.error_vector(values); + EXPECT(assert_equal(expected_whitened, actual_whitened)); + + double expected_error = 0.0; + double actual_error = factor.error(values); + DOUBLES_EQUAL(expected_error, actual_error, 1e-10); +} + +/* ************************************************************************* */ +TEST(LinearEquality, matrices_NULL) +{ + // Make sure everything works with NULL noise model + LinearEquality factor(simple::terms, simple::b, 0); + + Matrix AExpected(3, 9); + AExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << AExpected, rhsExpected; + + // Whitened Jacobian + EXPECT(assert_equal(AExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(AExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); +} + +/* ************************************************************************* */ +TEST(LinearEquality, matrices) +{ + // And now witgh a non-unit noise model + LinearEquality factor(simple::terms, simple::b, 0); + + Matrix jacobianExpected(3, 9); + jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second; + Vector rhsExpected = simple::b; + Matrix augmentedJacobianExpected(3, 10); + augmentedJacobianExpected << jacobianExpected, rhsExpected; + + Matrix augmentedHessianExpected = + augmentedJacobianExpected.transpose() * simple::noise->R().transpose() + * simple::noise->R() * augmentedJacobianExpected; + + // Whitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobian().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian())); + + // Unwhitened Jacobian + EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first)); + EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second)); + EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted())); +} + +/* ************************************************************************* */ +TEST(LinearEquality, operators ) +{ + Matrix I = eye(2); + Vector b = (Vector(2) << 0.2,-0.1).finished(); + LinearEquality lf(1, -I, 2, I, b, 0); + + VectorValues c; + c.insert(1, (Vector(2) << 10.,20.).finished()); + c.insert(2, (Vector(2) << 30.,60.).finished()); + + // test A*x + Vector expectedE = (Vector(2) << 20.,40.).finished(); + Vector actualE = lf * c; + EXPECT(assert_equal(expectedE, actualE)); + + // test A^e + VectorValues expectedX; + expectedX.insert(1, (Vector(2) << -20.,-40.).finished()); + expectedX.insert(2, (Vector(2) << 20., 40.).finished()); + VectorValues actualX = VectorValues::Zero(expectedX); + lf.transposeMultiplyAdd(1.0, actualE, actualX); + EXPECT(assert_equal(expectedX, actualX)); + + // test gradient at zero + Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); + VectorValues expectedG; + expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); + expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); + VectorValues actualG = lf.gradientAtZero(); + EXPECT(assert_equal(expectedG, actualG)); +} + +/* ************************************************************************* */ +TEST(LinearEquality, default_error ) +{ + LinearEquality f; + double actual = f.error(VectorValues()); + DOUBLES_EQUAL(0.0, actual, 1e-15); +} + +//* ************************************************************************* */ +TEST(LinearEquality, empty ) +{ + // create an empty factor + LinearEquality f; + EXPECT(f.empty()); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp new file mode 100644 index 000000000..8fca61ca4 --- /dev/null +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -0,0 +1,311 @@ +/* ---------------------------------------------------------------------------- + + * 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 testQPSolver.cpp + * @brief Test simple QP solver for a linear inequality constraint + * @date Apr 10, 2014 + * @author Duy-Nguyen Ta + */ + +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::symbol_shorthand; + +const Matrix One = ones(1,1); + +/* ************************************************************************* */ +// Create test graph according to Forst10book_pg171Ex5 +QP createTestCase() { + QP qp; + + // Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 + 5 + // Note the Hessian encodes: + // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f + // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 + qp.cost.push_back( + HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), + 2.0 * ones(1, 1), zero(1), 10.0)); + + // Inequality constraints + qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2 + + return qp; +} + +TEST(QPSolver, TestCase) { + VectorValues values; + double x1 = 5, x2 = 7; + values.insert(X(1), x1 * ones(1, 1)); + values.insert(X(2), x2 * ones(1, 1)); + QP qp = createTestCase(); + DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9); + DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9); +} + +TEST(QPSolver, constraintsAux) { + QP qp = createTestCase(); + + QPSolver solver(qp); + + VectorValues lambdas; + lambdas.insert(0, (Vector(1) << -0.5).finished()); + lambdas.insert(1, (Vector(1) << 0.0).finished()); + lambdas.insert(2, (Vector(1) << 0.3).finished()); + lambdas.insert(3, (Vector(1) << 0.1).finished()); + int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas); + LONGS_EQUAL(2, factorIx); + + VectorValues lambdas2; + lambdas2.insert(0, (Vector(1) << -0.5).finished()); + lambdas2.insert(1, (Vector(1) << 0.0).finished()); + lambdas2.insert(2, (Vector(1) << -0.3).finished()); + lambdas2.insert(3, (Vector(1) << -0.1).finished()); + int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2); + LONGS_EQUAL(-1, factorIx2); +} + +/* ************************************************************************* */ +// Create a simple test graph with one equality constraint +QP createEqualityConstrainedTest() { + QP qp; + + // Objective functions x1^2 + x2^2 + // Note the Hessian encodes: + // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f + // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 + qp.cost.push_back( + HessianFactor(X(1), X(2), 2.0 * ones(1, 1), zeros(1, 1), zero(1), + 2.0 * ones(1, 1), zero(1), 0.0)); + + // Equality constraints + // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector + Matrix A1 = (Matrix(1, 1) << 1).finished(); + Matrix A2 = (Matrix(1, 1) << 1).finished(); + Vector b = -(Vector(1) << 1).finished(); + qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); + + return qp; +} + +TEST(QPSolver, dual) { + QP qp = createEqualityConstrainedTest(); + + // Initials values + VectorValues initialValues; + initialValues.insert(X(1), ones(1)); + initialValues.insert(X(2), ones(1)); + + QPSolver solver(qp); + + GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph( + qp.inequalities, initialValues); + VectorValues dual = dualGraph->optimize(); + VectorValues expectedDual; + expectedDual.insert(0, (Vector(1) << 2.0).finished()); + CHECK(assert_equal(expectedDual, dual, 1e-10)); +} + +/* ************************************************************************* */ +TEST(QPSolver, indentifyActiveConstraints) { + QP qp = createTestCase(); + QPSolver solver(qp); + + VectorValues currentSolution; + currentSolution.insert(X(1), zero(1)); + currentSolution.insert(X(2), zero(1)); + + LinearInequalityFactorGraph workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); + + CHECK(!workingSet.at(0)->active()); // inactive + CHECK(workingSet.at(1)->active()); // active + CHECK(workingSet.at(2)->active()); // active + CHECK(!workingSet.at(3)->active()); // inactive + + VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 0.0).finished()); + expectedSolution.insert(X(2), (Vector(1) << 0.0).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-100)); + +} + +/* ************************************************************************* */ +TEST(QPSolver, iterate) { + QP qp = createTestCase(); + QPSolver solver(qp); + + VectorValues currentSolution; + currentSolution.insert(X(1), zero(1)); + currentSolution.insert(X(2), zero(1)); + + std::vector expectedSolutions(4), expectedDuals(4); + expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished()); + expectedSolutions[0].insert(X(2), (Vector(1) << 0.0).finished()); + expectedDuals[0].insert(1, (Vector(1) << 3).finished()); + expectedDuals[0].insert(2, (Vector(1) << 0).finished()); + + expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished()); + expectedSolutions[1].insert(X(2), (Vector(1) << 0.0).finished()); + expectedDuals[1].insert(3, (Vector(1) << 1.5).finished()); + + expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished()); + expectedSolutions[2].insert(X(2), (Vector(1) << 0.75).finished()); + + expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); + expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); + + LinearInequalityFactorGraph workingSet = + solver.identifyActiveConstraints(qp.inequalities, currentSolution); + + QPState state(currentSolution, VectorValues(), workingSet, false); + + int it = 0; + while (!state.converged) { + state = solver.iterate(state); + // These checks will fail because the expected solutions obtained from + // Forst10book do not follow exactly what we implemented from Nocedal06book. + // Specifically, we do not re-identify active constraints and + // do not recompute dual variables after every step!!! +// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10)); +// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10)); + it++; + } + + CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10)); +} + +/* ************************************************************************* */ + +TEST(QPSolver, optimizeForst10book_pg171Ex5) { + QP qp = createTestCase(); + QPSolver solver(qp); + VectorValues initialValues; + initialValues.insert(X(1), zero(1)); + initialValues.insert(X(2), zero(1)); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 1.5).finished()); + expectedSolution.insert(X(2), (Vector(1) << 0.5).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-100)); +} + +/* ************************************************************************* */ +// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html +QP createTestMatlabQPEx() { + QP qp; + + // Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2 + // Note the Hessian encodes: + // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f + // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 + qp.cost.push_back( + HessianFactor(X(1), X(2), 1.0 * ones(1, 1), -ones(1, 1), 2.0 * ones(1), + 2.0 * ones(1, 1), 6 * ones(1), 1000.0)); + + // Inequality constraints + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 + qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2 + qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3 + qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0 + + return qp; +} + +TEST(QPSolver, optimizeMatlabEx) { + QP qp = createTestMatlabQPEx(); + QPSolver solver(qp); + VectorValues initialValues; + initialValues.insert(X(1), zero(1)); + initialValues.insert(X(2), zero(1)); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); + expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-7)); +} + +/* ************************************************************************* */ +// Create test graph as in Nocedal06book, Ex 16.4, pg. 475 +QP createTestNocedal06bookEx16_4() { + QP qp; + + qp.cost.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); + qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); + + // Inequality constraints + qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1)); + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2)); + qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3)); + qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4)); + + return qp; +} + +TEST(QPSolver, optimizeNocedal06bookEx16_4) { + QP qp = createTestNocedal06bookEx16_4(); + QPSolver solver(qp); + VectorValues initialValues; + initialValues.insert(X(1), (Vector(1) << 2.0).finished()); + initialValues.insert(X(2), zero(1)); + + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 1.4).finished()); + expectedSolution.insert(X(2), (Vector(1) << 1.7).finished()); + CHECK(assert_equal(expectedSolution, solution, 1e-7)); +} + +/* ************************************************************************* */ + +TEST(QPSolver, failedSubproblem) { + QP qp; + qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2))); + qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); + qp.inequalities.push_back( + LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); + + VectorValues expected; + expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); + + VectorValues initialValues; + initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); + + QPSolver solver(qp); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); +// graph.print("Graph: "); +// solution.print("Solution: "); + CHECK(assert_equal(expected, solution, 1e-7)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + 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/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/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index 45e294c3d..7c2f9d9b9 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -16,7 +16,7 @@ * @brief Test meta-programming constructs for Expressions */ -#include +#include #include #include #include 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/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 05260521e..4691da384 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -45,7 +45,7 @@ static boost::shared_ptr Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, static double rankTol = 1.0; static double linThreshold = -1.0; -static bool manageDegeneracy = true; +// static bool manageDegeneracy = true; // Create a noise model for the pixel error static SharedNoiseModel model(noiseModel::Unit::Create(3)); 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 ad2c0767a..72bc4e8e3 100644 --- a/tests/testPreconditioner.cpp +++ b/tests/testPreconditioner.cpp @@ -28,86 +28,99 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -static GaussianFactorGraph createSimpleGaussianFactorGraph() { - GaussianFactorGraph fg; - SharedDiagonal unit2 = noiseModel::Unit::Create(2); - // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg += JacobianFactor(2, 10*eye(2), -1.0*ones(2), unit2); - // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg += JacobianFactor(2, -10*eye(2), 0, 10*eye(2), Vector2(2.0, -1.0), unit2); - // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg += JacobianFactor(2, -5*eye(2), 1, 5*eye(2), Vector2(0.0, 1.0), unit2); - // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg += JacobianFactor(0, -5*eye(2), 1, 5*eye(2), Vector2(-1.0, 1.5), unit2); - return fg; +TEST( PCGsolver, verySimpleLinearSystem) { + + // Ax = [4 1][u] = [1] x0 = [2] + // [1 3][v] [2] [1] + // + // exact solution x = [1/11, 7/11]'; + // + + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< 4, 1, 1, 3).finished(), (Vector(2) << 1,2 ).finished(), noiseModel::Unit::Create(2)); + + // Exact solution already known + VectorValues exactSolution; + exactSolution.insert(0, (Vector(2) << 1./11., 7./11.).finished()); + //exactSolution.print("Exact"); + + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(exactSolution, deltaDirect, 1e-7)); + //deltaDirect.print("Direct"); + + // Solve the system using Preconditioned Conjugate Gradient solver + // Common PCG parameters + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + //pcg->setVerbosity("ERROR"); + + // With Dummy preconditioner + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGDummy = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(exactSolution, deltaPCGDummy, 1e-7)); + //deltaPCGDummy.print("PCG Dummy"); + + // With Block-Jacobi preconditioner + pcg->preconditioner_ = boost::make_shared(); + // It takes more than 1000 iterations for this test + pcg->setMaxIterations(1500); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); + + EXPECT(assert_equal(exactSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); } /* ************************************************************************* */ -// Copy of BlockJacobiPreconditioner::build -std::vector buildBlocks( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo) -{ - const size_t n = keyInfo.size(); - std::vector dims_ = keyInfo.colSpec(); +TEST(PCGSolver, simpleLinearSystem) { + // Create a Gaussian Factor Graph + GaussianFactorGraph simpleGFG; + //SharedDiagonal unit2 = noiseModel::Unit::Create(2); + SharedDiagonal unit2 = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.3)); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << -1, -1).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -10, 0, 0, -10).finished(), 0, (Matrix(2,2)<< 10, 0, 0, 10).finished(), (Vector(2) << 2, -1).finished(), unit2); + simpleGFG += JacobianFactor(2, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << 0, 1).finished(), unit2); + simpleGFG += JacobianFactor(0, (Matrix(2,2)<< -5, 0, 0, -5).finished(), 1, (Matrix(2,2)<< 5, 0, 0, 5).finished(), (Vector(2) << -1, 1.5).finished(), unit2); + 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"); - /* prepare the buffer of block diagonals */ - std::vector blocks; blocks.reserve(n); + // 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"); - /* allocate memory for the factorization of block diagonals */ - size_t nnz = 0; - for ( size_t i = 0 ; i < n ; ++i ) { - const size_t dim = dims_[i]; - blocks.push_back(Matrix::Zero(dim, dim)); - // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ; - nnz += dim*dim; - } + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); + deltaDirect.print("Direct"); - /* compute the block diagonal by scanning over the factors */ - BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { - if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { - for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { - const KeyInfoEntry &entry = keyInfo.find(*it)->second; - const Matrix &Ai = jf->getA(it); - blocks[entry.index()] += (Ai.transpose() * Ai); - } - } - else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { - for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) { - const KeyInfoEntry &entry = keyInfo.find(*it)->second; - const Matrix &Hii = hf->info(it, it).selfadjointView(); - blocks[entry.index()] += Hii; - } - } - else { - throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); - } - } + // Solve the system using Preconditioned Conjugate Gradient solver + // Common PCG parameters + gtsam::PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->setMaxIterations(500); + pcg->setEpsilon_abs(0.0); + pcg->setEpsilon_rel(0.0); + pcg->setVerbosity("ERROR"); - return blocks; -} + // 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"); -/* ************************************************************************* */ -TEST( Preconditioner, buildBlocks ) { - // Create simple Gaussian factor graph and initial values - GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); - Values initial; - initial.insert(0,Point2(4, 5)); - initial.insert(1,Point2(0, 1)); - initial.insert(2,Point2(-5, 7)); + // 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"); - // Expected Hessian block diagonal matrices - std::map expectedHessian =gfg.hessianBlockDiagonal(); - - // Actual Hessian block diagonal matrices from BlockJacobiPreconditioner::build - std::vector actualHessian = buildBlocks(gfg, KeyInfo(gfg)); - - // Compare the number of block diagonal matrices - EXPECT_LONGS_EQUAL(expectedHessian.size(), actualHessian.size()); - - // Compare the values of matrices - std::map::const_iterator it1 = expectedHessian.begin(); - std::vector::const_iterator it2 = actualHessian.begin(); - for(; it1!=expectedHessian.end(); it1++, it2++) - EXPECT(assert_equal(it1->second, *it2)); } /* ************************************************************************* */ 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/gtsam_unstable/timing/timeAdaptAutoDiff.cpp b/timing/timeAdaptAutoDiff.cpp similarity index 97% rename from gtsam_unstable/timing/timeAdaptAutoDiff.cpp rename to timing/timeAdaptAutoDiff.cpp index 437067697..edfd420ef 100644 --- a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -19,7 +19,7 @@ #include "timeLinearize.h" #include #include -#include +#include #include #include #include diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/timing/timeCameraExpression.cpp similarity index 95% rename from gtsam_unstable/timing/timeCameraExpression.cpp rename to timing/timeCameraExpression.cpp index f872ce78a..208c991fd 100644 --- a/gtsam_unstable/timing/timeCameraExpression.cpp +++ b/timing/timeCameraExpression.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include #include #include @@ -32,7 +32,7 @@ using namespace gtsam; boost::shared_ptr fixedK(new Cal3_S2()); Point2 myProject(const Pose3& pose, const Point3& point, - OptionalJacobian<2, 6> H1, OptionalJacobian<2, 3> H2) { + OptionalJacobian<2,6> H1, OptionalJacobian<2,3> H2) { PinholeCamera camera(pose, *fixedK); return camera.project(point, H1, H2, boost::none); } diff --git a/timing/timeIncremental.cpp b/timing/timeIncremental.cpp index 6f2909b7a..d82ef9442 100644 --- a/timing/timeIncremental.cpp +++ b/timing/timeIncremental.cpp @@ -14,15 +14,15 @@ * @author Richard Roberts */ -#include #include -#include #include #include #include +#include #include #include #include +#include #include #include @@ -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) @@ -97,7 +97,7 @@ int main(int argc, char *argv[]) { // cout << "Initializing " << 0 << endl; newVariables.insert(0, Pose()); // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); } while(nextMeasurement < measurements.size()) { diff --git a/gtsam_unstable/timing/timeLinearize.h b/timing/timeLinearize.h similarity index 100% rename from gtsam_unstable/timing/timeLinearize.h rename to timing/timeLinearize.h diff --git a/gtsam_unstable/timing/timeOneCameraExpression.cpp b/timing/timeOneCameraExpression.cpp similarity index 92% rename from gtsam_unstable/timing/timeOneCameraExpression.cpp rename to timing/timeOneCameraExpression.cpp index 4cb655825..962e7ee21 100644 --- a/gtsam_unstable/timing/timeOneCameraExpression.cpp +++ b/timing/timeOneCameraExpression.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include "timeLinearize.h" using namespace std; 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; } /* ************************************************************************* */ diff --git a/gtsam_unstable/timing/timeSFMExpressions.cpp b/timing/timeSFMExpressions.cpp similarity index 95% rename from gtsam_unstable/timing/timeSFMExpressions.cpp rename to timing/timeSFMExpressions.cpp index 678e4db60..cfb8e0dc6 100644 --- a/gtsam_unstable/timing/timeSFMExpressions.cpp +++ b/timing/timeSFMExpressions.cpp @@ -16,8 +16,8 @@ * @date October 3, 2014 */ -#include -#include +#include +#include #include #include diff --git a/timing/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp index d15ccfaf4..c4196f414 100644 --- a/timing/timeiSAM2Chain.cpp +++ b/timing/timeiSAM2Chain.cpp @@ -39,7 +39,7 @@ typedef Pose2 Pose; typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; -noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(Pose::Dim()); +noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3); int main(int argc, char *argv[]) { @@ -61,10 +61,10 @@ int main(int argc, char *argv[]) { gttic_(Create_measurements); if(step == 0) { // Add prior - newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + newFactors.add(PriorFactor(0, Pose(), noiseModel::Unit::Create(3))); newVariables.insert(0, Pose()); } else { - Vector eta = Vector::Random(Pose::Dim()) * 0.1; + Vector eta = Vector::Random(3) * 0.1; Pose2 between = Pose().retract(eta); // Add between newFactors.add(BetweenFactor(step-1, step, between, model));