diff --git a/.cproject b/.cproject index d081a750a6..6fe436adcc 100644 --- a/.cproject +++ b/.cproject @@ -510,34 +510,26 @@ true true - - make - -j5 - SFMExampleExpressions.run - true - true - true - - + make -j5 - Pose2SLAMExampleExpressions.run + testInvDepthCamera3.run true true true - + make -j5 - testInvDepthCamera3.run + testTriangulation.run true true true - + make - -j5 - testTriangulation.run + -j4 + testEvent.run true true true @@ -856,6 +848,22 @@ true true + + make + -j4 + testSmartStereoProjectionPoseFactor.run + true + true + true + + + make + -j4 + testTOAFactor.run + true + true + true + make -j5 @@ -960,38 +968,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 @@ -1583,6 +1559,14 @@ false true + + make + -j4 + testLabeledSymbol.run + true + true + true + make -j2 @@ -1994,6 +1978,14 @@ true true + + make + -j2 VERBOSE=1 + check.navigation + true + false + true + make -j2 @@ -2177,6 +2169,14 @@ true true + + make + -j4 + testSO3.run + true + true + true + make -j2 @@ -2537,6 +2537,14 @@ true true + + make + -j4 + testOptionalJacobian.run + true + true + true + make -j5 @@ -2766,14 +2774,6 @@ true true - - make - -j5 - testExpressionFactor.run - true - true - true - make -j5 @@ -2782,30 +2782,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 @@ -2950,6 +2926,14 @@ true true + + make + -j4 + testRangeFactor.run + true + true + true + make -j2 @@ -3126,6 +3110,22 @@ true true + + make + -j4 + Pose2SLAMExampleExpressions.run + true + true + true + + + make + -j4 + SFMExampleExpressions.run + true + true + true + make -j5 @@ -3174,6 +3174,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 @@ -3222,6 +3246,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 diff --git a/.gitignore b/.gitignore index e583e3ab08..d46bddd10f 100644 --- a/.gitignore +++ b/.gitignore @@ -1,8 +1,8 @@ /build* -/doc* *.pyc *.DS_Store /examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/pose2example-rewritten.txt /examples/Data/pose3example-rewritten.txt -/.settings/ +*.txt.user +*.txt.user.6d59f0c diff --git a/.settings/.gitignore b/.settings/.gitignore new file mode 100644 index 0000000000..faa6d29912 --- /dev/null +++ b/.settings/.gitignore @@ -0,0 +1 @@ +/org.eclipse.cdt.codan.core.prefs diff --git a/.settings/org.eclipse.cdt.core.prefs b/.settings/org.eclipse.cdt.core.prefs new file mode 100644 index 0000000000..b700d0a818 --- /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 3ea6a63185..ff1f1b692f 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -80,6 +80,12 @@ class Test 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 0000000000..3c10451be4 --- /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 623b1ff326..679af5a2fb 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 0d5d9db30b..9c06a5d287 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 a41b71045b..0493db6809 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/cmake/GtsamBuildTypes.cmake b/cmake/GtsamBuildTypes.cmake index 2f11df94e8..1bead58d89 100644 --- a/cmake/GtsamBuildTypes.cmake +++ b/cmake/GtsamBuildTypes.cmake @@ -54,7 +54,7 @@ if(NOT FIRST_PASS_DONE) endif() # Clang on Mac uses a template depth that is less than standard and is too small -if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") +if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024") endif() diff --git a/doc/Mathematica/dexpInvL_SE2.nb b/doc/Mathematica/dexpInvL_SE2.nb new file mode 100644 index 0000000000..d159327685 --- /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/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp similarity index 82% rename from gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp rename to examples/Pose2SLAMExampleExpressions.cpp index 3a3b97b776..92f94c3f31 100644 --- a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -18,8 +18,8 @@ */ // The two new headers that allow using our Automatic Differentiation Expression framework -#include -#include +#include +#include // Header order is close to far #include @@ -35,26 +35,26 @@ using namespace gtsam; int main(int argc, char** argv) { // 1. Create a factor graph container and add factors to it - NonlinearFactorGraph graph; + ExpressionFactorGraph graph; // Create Expressions for unknowns Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); // 2a. Add a prior on the first pose, setting it to the origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); - graph.push_back(ExpressionFactor(priorNoise, Pose2(0, 0, 0), x1)); + graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise); // For simplicity, we will use the same noise model for odometry and loop closures noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors - graph.push_back(ExpressionFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4))); - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x4,x5))); + graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model); + graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model); + graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - graph.push_back(ExpressionFactor(model, Pose2(2, 0, M_PI_2), between(x5,x2))); + graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution diff --git a/gtsam_unstable/examples/SFMExampleExpressions.cpp b/examples/SFMExampleExpressions.cpp similarity index 64% rename from gtsam_unstable/examples/SFMExampleExpressions.cpp rename to examples/SFMExampleExpressions.cpp index 941f3dcb8d..e9c9e920d3 100644 --- a/gtsam_unstable/examples/SFMExampleExpressions.cpp +++ b/examples/SFMExampleExpressions.cpp @@ -23,15 +23,12 @@ */ // The two new headers that allow using our Automatic Differentiation Expression framework -#include -#include +#include +#include // Header order is close to far #include -#include -#include #include -#include #include #include #include @@ -40,27 +37,29 @@ using namespace std; using namespace gtsam; +using namespace noiseModel; /* ************************************************************************* */ int main(int argc, char* argv[]) { Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0); - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + Isotropic::shared_ptr measurementNoise = Isotropic::Sigma(2, 1.0); // one pixel in u and v // Create the set of ground-truth landmarks and poses vector points = createPoints(); vector poses = createPoses(); // Create a factor graph - NonlinearFactorGraph graph; + ExpressionFactorGraph graph; // Specify uncertainty on first pose prior - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); + Vector6 sigmas; sigmas << Vector3(0.3,0.3,0.3), Vector3(0.1,0.1,0.1); + Diagonal::shared_ptr poseNoise = Diagonal::Sigmas(sigmas); // Here we don't use a PriorFactor but directly the ExpressionFactor class - // The object x0 is an Expression, and we create a factor wanting it to be equal to poses[0] + // x0 is an Expression, and we create a factor wanting it to be equal to poses[0] Pose3_ x0('x',0); - graph.push_back(ExpressionFactor(poseNoise, poses[0], x0)); + graph.addExpressionFactor(x0, poses[0], poseNoise); // We create a constant Expression for the calibration here Cal3_S2_ cK(K); @@ -74,25 +73,26 @@ int main(int argc, char* argv[]) { // Below an expression for the prediction of the measurement: Point3_ p('l', j); Point2_ prediction = uncalibrate(cK, project(transform_to(x, p))); - // Again, here we use a ExpressionFactor - graph.push_back(ExpressionFactor(measurementNoise, measurement, prediction)); + // Again, here we use an ExpressionFactor + graph.addExpressionFactor(prediction, measurement, measurementNoise); } } // Add prior on first point to constrain scale, again with ExpressionFactor - noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); - graph.push_back(ExpressionFactor(pointNoise, points[0], Point3_('l', 0))); + Isotropic::shared_ptr pointNoise = Isotropic::Sigma(3, 0.1); + graph.addExpressionFactor(Point3_('l', 0), points[0], pointNoise); // Create perturbed initial - Values initialEstimate; + Values initial; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initial.insert(Symbol('x', i), poses[i].compose(delta)); for (size_t j = 0; j < points.size(); ++j) - initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); - cout << "initial error = " << graph.error(initialEstimate) << endl; + initial.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); + cout << "initial error = " << graph.error(initial) << endl; /* Optimize the graph and print results */ - Values result = DoglegOptimizer(graph, initialEstimate).optimize(); + Values result = DoglegOptimizer(graph, initial).optimize(); cout << "final error = " << graph.error(result) << endl; return 0; diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 3dd978ee33..923b0b9de8 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) @@ -295,7 +295,7 @@ void runIncremental() NonlinearFactorGraph newFactors; Values newVariables; - newFactors.push_back(boost::make_shared >(firstPose, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + newFactors.push_back(boost::make_shared >(firstPose, Pose(), noiseModel::Unit::Create(3))); newVariables.insert(firstPose, Pose()); isam2.update(newFactors, newVariables); @@ -474,7 +474,7 @@ void runBatch() cout << "Creating batch optimizer..." << endl; NonlinearFactorGraph measurements = datasetMeasurements; - measurements.push_back(boost::make_shared >(0, Pose(), noiseModel::Unit::Create(Pose::Dim()))); + measurements.push_back(boost::make_shared >(0, Pose(), noiseModel::Unit::Create(3))); gttic_(Create_optimizer); GaussNewtonParams params; diff --git a/gtsam.h b/gtsam.h index 96d51117ae..1fbc0f907a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -156,12 +156,6 @@ virtual class Value { size_t dim() const; }; -class Vector3 { - Vector3(Vector v); -}; -class Vector6 { - Vector6(Vector v); -}; #include class LieScalar { // Standard constructors @@ -276,8 +270,6 @@ class Point2 { gtsam::Point2 between(const gtsam::Point2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Point2 retract(Vector v) const; Vector localCoordinates(const gtsam::Point2& p) const; @@ -348,8 +340,6 @@ class Point3 { gtsam::Point3 between(const gtsam::Point3& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Point3 retract(Vector v) const; Vector localCoordinates(const gtsam::Point3& p) const; @@ -386,8 +376,6 @@ class Rot2 { gtsam::Rot2 between(const gtsam::Rot2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Rot2 retract(Vector v) const; Vector localCoordinates(const gtsam::Rot2& p) const; @@ -439,8 +427,6 @@ class Rot3 { gtsam::Rot3 between(const gtsam::Rot3& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options gtsam::Rot3 retract(Vector v) const; Vector localCoordinates(const gtsam::Rot3& p) const; @@ -488,8 +474,6 @@ class Pose2 { gtsam::Pose2 between(const gtsam::Pose2& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Pose2 retract(Vector v) const; Vector localCoordinates(const gtsam::Pose2& p) const; @@ -537,10 +521,7 @@ class Pose3 { gtsam::Pose3 between(const gtsam::Pose3& p2) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::Pose3 retract(Vector v) const; - gtsam::Pose3 retractFirstOrder(Vector v) const; Vector localCoordinates(const gtsam::Pose3& T2) const; // Lie Group @@ -758,10 +739,6 @@ class CalibratedCamera { gtsam::CalibratedCamera retract(const Vector& d) const; Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; - // Group - gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const; - gtsam::CalibratedCamera inverse() const; - // Action on Point3 gtsam::Point2 project(const gtsam::Point3& point) const; static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint); @@ -1234,6 +1211,7 @@ class VectorValues { #include virtual class GaussianFactor { + gtsam::KeyVector keys() const; void print(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; @@ -1657,6 +1635,7 @@ class NonlinearFactorGraph { void push_back(gtsam::NonlinearFactor* factor); void add(gtsam::NonlinearFactor* factor); bool exists(size_t idx) const; + gtsam::KeySet keys() const; // NonlinearFactorGraph double error(const gtsam::Values& values) const; @@ -1727,6 +1706,7 @@ class Values { // void insert(size_t j, const gtsam::Value& value); // void update(size_t j, const gtsam::Value& val); // gtsam::Value at(size_t j) const; + void insert(size_t j, const gtsam::Point2& t); void insert(size_t j, const gtsam::Point3& t); void insert(size_t j, const gtsam::Rot2& t); @@ -1737,10 +1717,14 @@ class Values { void insert(size_t j, const gtsam::Cal3DS2& t); void insert(size_t j, const gtsam::Cal3Bundler& t); void insert(size_t j, const gtsam::EssentialMatrix& t); + void insert(size_t j, const gtsam::SimpleCamera& t); void insert(size_t j, const gtsam::imuBias::ConstantBias& t); void insert(size_t j, Vector t); void insert(size_t j, Matrix t); + // Fixed size version + void insertFixed(size_t j, Vector t, size_t n); + void update(size_t j, const gtsam::Point2& t); void update(size_t j, const gtsam::Point3& t); void update(size_t j, const gtsam::Rot2& t); @@ -1757,6 +1741,16 @@ class Values { template T at(size_t j); + + /// Fixed size versions, for n in 1..9 + void insertFixed(size_t j, Vector t, size_t n); + + /// Fixed size versions, for n in 1..9 + Vector atFixed(size_t j, size_t n); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; }; // Actually a FastList @@ -2153,7 +2147,7 @@ class NonlinearISAM { #include #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); T prior() const; @@ -2198,10 +2192,14 @@ typedef gtsam::RangeFactor RangeFactorPosePoint2; typedef gtsam::RangeFactor RangeFactorPosePoint3; typedef gtsam::RangeFactor RangeFactorPose2; typedef gtsam::RangeFactor RangeFactorPose3; -typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -typedef gtsam::RangeFactor RangeFactorSimpleCamera; + +// Commented out by Frank Dec 2014: not poses! +// If needed, we need a RangeFactor that takes a camera, extracts the pose +// Should be easy with Expressions +//typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +//typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +//typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +//typedef gtsam::RangeFactor RangeFactorSimpleCamera; #include @@ -2373,8 +2371,6 @@ class ConstantBias { gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; // Manifold - static size_t Dim(); - size_t dim() const; gtsam::imuBias::ConstantBias retract(Vector v) const; Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; @@ -2393,25 +2389,24 @@ class ConstantBias { }///\namespace imuBias #include -class PoseVelocity{ - PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity); +class PoseVelocityBias{ + PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias); }; class ImuFactorPreintegratedMeasurements { // Standard Constructor ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); - ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); + // ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); // Testable void print(string s) const; bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol); - Matrix measurementCovariance() const; - Matrix deltaRij() const; double deltaTij() const; + Matrix deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; - Vector biasHat() const; + Vector biasHatVector() const; Matrix delPdelBiasAcc() const; Matrix delPdelBiasOmega() const; Matrix delVdelBiasAcc() const; @@ -2419,10 +2414,11 @@ class ImuFactorPreintegratedMeasurements { Matrix delRdelBiasOmega() const; Matrix preintMeasCov() const; - // Standard Interface void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT); void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias, + Vector gravity, Vector omegaCoriolis) const; }; virtual class ImuFactor : gtsam::NonlinearFactor { @@ -2433,54 +2429,9 @@ virtual class ImuFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor); // Standard Interface gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; - gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias, - const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, - Vector gravity, Vector omegaCoriolis) const; -}; - -#include -class AHRSFactorPreintegratedMeasurements { - // Standard Constructor - AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); - AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs); - - // Testable - void print(string s) const; - bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); - - // get Data - Matrix measurementCovariance() const; - Matrix deltaRij() const; - double deltaTij() const; - Vector biasHat() const; - - // Standard Interface - void integrateMeasurement(Vector measuredOmega, double deltaT); - void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); - void resetIntegration() ; -}; - -virtual class AHRSFactor : gtsam::NonlinearFactor { - AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis); - AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis, - const gtsam::Pose3& body_P_sensor); - - // Standard Interface - gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; - Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, - Vector bias) const; - gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, - const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, - Vector omegaCoriolis) const; }; #include -class PoseVelocityBias{ - PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias); - }; class CombinedImuFactorPreintegratedMeasurements { // Standard Constructor CombinedImuFactorPreintegratedMeasurements( @@ -2500,39 +2451,74 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix biasOmegaCovariance, Matrix biasAccOmegaInit, bool use2ndOrderIntegration); - CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); + // 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; + Matrix deltaRij() const; Vector deltaPij() const; Vector deltaVij() const; - Vector biasHat() const; + Vector biasHatVector() const; Matrix delPdelBiasAcc() const; Matrix delPdelBiasOmega() const; Matrix delVdelBiasAcc() const; Matrix delVdelBiasOmega() const; Matrix delRdelBiasOmega() const; - Matrix PreintMeasCov() 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; - gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i, - const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, - Vector gravity, Vector omegaCoriolis); +}; + +#include +class AHRSFactorPreintegratedMeasurements { + // Standard Constructor + AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); + AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance); + AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs); + + // Testable + void print(string s) const; + bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol); + + // get Data + Matrix deltaRij() const; + double deltaTij() const; + Vector biasHat() const; + + // Standard Interface + void integrateMeasurement(Vector measuredOmega, double deltaT); + void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor); + void resetIntegration() ; +}; + +virtual class AHRSFactor : gtsam::NonlinearFactor { + AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, + const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis); + AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, + const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis, + const gtsam::Pose3& body_P_sensor); + + // Standard Interface + gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const; + Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, + Vector bias) const; + gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, + const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, + Vector omegaCoriolis) const; }; #include diff --git a/gtsam/3rdparty/metis/CMakeLists.txt b/gtsam/3rdparty/metis/CMakeLists.txt index fd9c7eaf7e..6ba17787fb 100644 --- a/gtsam/3rdparty/metis/CMakeLists.txt +++ b/gtsam/3rdparty/metis/CMakeLists.txt @@ -2,14 +2,14 @@ cmake_minimum_required(VERSION 2.8) project(METIS) # Add flags for currect directory and below -if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") +if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") add_definitions(-Wno-unused-variable) if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 5.0 OR CMAKE_CXX_COMPILER_VERSION VERSION_EQUAL 5.0) add_definitions(-Wno-sometimes-uninitialized) endif() endif() -if(NOT ("${CMAKE_C_COMPILER_ID}" MATCHES "MSVC" OR "${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC")) +if(NOT (${CMAKE_C_COMPILER_ID} MATCHES "MSVC" OR ${CMAKE_CXX_COMPILER_ID} MATCHES "MSVC")) #add_definitions(-Wno-unknown-pragmas) endif() diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h deleted file mode 100644 index 3cc6a041c5..0000000000 --- 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 6c109675d5..c67f7dd61e 100644 --- a/gtsam/base/GenericValue.h +++ b/gtsam/base/GenericValue.h @@ -19,78 +19,18 @@ #pragma once -#include +#include #include + +#include +#include + #include #include +#include // operator typeid namespace gtsam { -// To play as a GenericValue, we need the following traits -namespace traits { - -// trait to wrap the default equals of types -template -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 +46,8 @@ class GenericValue: public Value { T value_; ///< The wrapped value public: + // Only needed for serialization. + GenericValue(){} /// Construct from value GenericValue(const T& value) : @@ -131,31 +73,124 @@ class GenericValue: public Value { // 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); + std::cout << "(" << typeid(T).name() << ") "; + traits::Print(value_, str); } - // Serialization below: - - 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_); - } - -protected: - - // Assignment operator for this class not needed since GenericValue is an abstract class + /** + * 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; + } + + /** + * 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 + } + + /** + * Clone this value (normal clone on the heap, delete with 'delete' operator) + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(*this); + } + + /// 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 5b2574ee3b..aec4b5f1c4 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 { +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; + + 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: - static T concept_check(const T& t) { - /** assignment */ - T t2 = t; + 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; + } - /** identity */ - T identity = T::identity(); + flavor_tag flavor; + G e, g, h; + bool b; +}; - /** compose with another object */ - T compose_ret = identity.compose(t2); +/// 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); +} - /** invert the object and yield a new one */ - T inverse_ret = compose_ret.inverse(); +/// 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;} - return inverse_ret; - } -}; +/// 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 +} // \ 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 7a9d32249e..bf2056cc87 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,235 @@ namespace gtsam { +/// A CRTP helper class that implements Lie group methods +/// Prerequisites: methods operator*, inverse, and AdjointMap, as well as a +/// ChartAtOrigin struct that will be used to define the manifold Chart +/// To use, simply derive, but also say "using LieGroup::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,H2 ? &D_g_v : 0); + Class h = compose(g,H1,H2); + if (H2) *H2 = (*H2) * D_g_v; + return h; + } + + TangentVector localCoordinates(const Class& g, // + ChartJacobian H1, ChartJacobian H2 = boost::none) const { + Class h = between(g,H1,H2); + Jacobian D_v_h; + TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); + if (H1) *H1 = D_v_h * (*H1); + if (H2) *H2 = D_v_h * (*H2); + return v; + } + +}; + +/// tag to assert a type is a Lie group +struct lie_group_tag: public manifold_tag, public group_tag {}; + +namespace internal { + +/// A helper class that implements the traits interface for GTSAM lie groups. +/// To use this for your gtsam type, define: +/// template<> struct traits : 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();} + /// @} + + /// @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, + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { + return origin.localCoordinates(other, Horigin, Hother); + } + + static Class Retract(const Class& origin, const TangentVector& v, + ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { + return origin.retract(v, Horigin, Hv); + } + + /// @} + + /// @name Lie Group + /// @{ + + static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) { + return Class::Logmap(m, Hm); + } + + static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) { + return Class::Expmap(v, Hv); + } + + static Class Compose(const Class& m1, const Class& m2, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + return m1.compose(m2, H1, H2); + } + + static Class Between(const Class& m1, const Class& m2, // + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + return m1.between(m2, H1, H2); + } + + static Class Inverse(const Class& m, // + ChartJacobian H = boost::none) { + return m.inverse(H); + } + + /// @} + +}; + +} // \ namepsace internal + /** * These core global functions can be specialized by new Lie types * for better performance. */ /** Compute l0 s.t. l2=l1*l0 */ -template -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) { - - /** assignment */ - T t2 = 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; - /** - * Returns dimensionality of the tangent space - */ - size_t dim_ret = t.dim(); + 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)"); - /** 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 +302,5 @@ T expm(const Vector& x, int K=7) { * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable */ -#define GTSAM_CONCEPT_LIE_INST(T) \ - template class gtsam::ManifoldConcept; \ - 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 4c250b3f26..90b7207a23 100644 --- a/gtsam/base/LieMatrix.h +++ b/gtsam/base/LieMatrix.h @@ -19,16 +19,19 @@ #include +#ifdef _MSC_VER +#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.") +#else #warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead." -#include -#include -#include +#endif + +#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. */ @@ -36,13 +39,17 @@ struct LieMatrix : public Matrix { /// @name Constructors /// @{ + enum { dimension = Eigen::Dynamic }; - /** default constructor - should be unnecessary */ + /** default constructor - only for serialize */ LieMatrix() {} /** initialize from a normal matrix */ LieMatrix(const Matrix& v) : Matrix(v) {} + template + LieMatrix(const M& v) : Matrix(v) {} + // Currently TMP constructor causes ICE on MSVS 2013 #if (_MSC_VER < 1800) /** initialize from a fixed size normal vector */ @@ -65,102 +72,57 @@ struct LieMatrix : public Matrix { inline bool equals(const LieMatrix& expected, double tol=1e-5) const { return gtsam::equal_with_abs_tol(matrix(), expected.matrix(), tol); } - + /// @} /// @name Standard Interface /// @{ - /** get the underlying vector */ + /** get the underlying matrix */ inline Matrix matrix() const { return static_cast(*this); } - + /// @} - /// @name Manifold interface - /// @{ - /** Returns dimensionality of the tangent space */ - inline size_t dim() const { return this->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())); - } + /// @name Group + /// @{ + LieMatrix compose(const LieMatrix& q) { return (*this)+q;} + LieMatrix between(const LieMatrix& q) { return q-(*this);} + LieMatrix inverse() { return -(*this);} + /// @} - /** @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; - return result; - } + /// @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 Group interface + + /// @name VectorSpace requirements /// @{ + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return size(); } + + /** 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; + } + /** 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: @@ -176,17 +138,20 @@ struct LieMatrix : public Matrix { }; -// Define GTSAM traits -namespace traits { template<> -struct is_manifold : public boost::true_type { -}; +struct traits : public internal::VectorSpace { + + // 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()); + } -template<> -struct dimension : public Dynamic { }; -} - } // \namespace gtsam diff --git a/gtsam/base/LieScalar.h b/gtsam/base/LieScalar.h index c684583644..9f6c56b287 100644 --- a/gtsam/base/LieScalar.h +++ b/gtsam/base/LieScalar.h @@ -17,10 +17,14 @@ #pragma once -#warning "LieScalar.h is deprecated. Please use double/float instead." +#ifdef _MSC_VER +#pragma message("LieScalar.h is deprecated. Please use double/float instead.") +#else + #warning "LieScalar.h is deprecated. Please use double/float instead." +#endif + #include -#include -#include +#include namespace gtsam { @@ -31,11 +35,13 @@ namespace gtsam { */ struct GTSAM_EXPORT LieScalar { + enum { dimension = 1 }; + /** default constructor */ LieScalar() : d_(0.0) {} /** wrap a double */ - explicit LieScalar(double d) : d_(d) {} + /*explicit*/ LieScalar(double d) : d_(d) {} /** access the underlying value */ double value() const { return d_; } @@ -43,93 +49,43 @@ namespace gtsam { /** Automatic conversion to underlying value */ operator double() const { return d_; } - /** print @param name optional string naming the object */ - void print(const std::string& name="") const; + /** convert vector */ + Vector1 vector() const { Vector1 v; v< 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(); } + Vector1 localCoordinates(const LieScalar& q) { return between(q).vector();} + LieScalar retract(const Vector1& v) {return compose(LieScalar(v[0]));} + /// @} - /// 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 f6cae28e2e..84afdabc8c 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 94ba775cf7..c2003df3c8 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -17,26 +17,34 @@ #pragma once +#ifdef _MSC_VER +#pragma message("LieVector.h is deprecated. Please use Eigen::Vector instead.") +#else #warning "LieVector.h is deprecated. Please use Eigen::Vector instead." -#include -#include -#include +#endif + +#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 */ @@ -50,75 +58,51 @@ struct LieVector : public Vector { /** constructor with size and initial data, row order ! */ GTSAM_EXPORT LieVector(size_t m, const double* const data); - /** get the underlying vector */ - Vector vector() const { - return static_cast(*this); - } - - /** print @param name optional string naming the object */ + /// @name Testable + /// @{ 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); } + /// @} + + /// @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 + /// @{ - // Manifold requirements + /** get the underlying vector */ + Vector vector() const { + return static_cast(*this); + } /** 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: @@ -131,17 +115,8 @@ struct LieVector : public Vector { } }; -// 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 1190822ed7..6998b3b18e 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 { -// is manifold, by default this is false -template -struct is_manifold: public boost::false_type { -}; + enum { dim = Class::dimension }; -// 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 { -}; + Class p, q; + Eigen::Matrix v; + OptionalJacobian Hp, Hq, Hv; -/** - * 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; + BOOST_CONCEPT_USAGE(HasManifoldPrereqs) { + v = p.localCoordinates(q); + q = p.retract(v); } }; -// 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 +/// Extra manifold traits for fixed-dimension types +template +struct ManifoldImpl { + // Compile-time dimensionality + static int GetDimension(const Class&) { + return N; + } }; -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(); - } -}; -namespace traits { -// populate default traits -template struct is_chart > : public boost::true_type { -}; -template struct dimension > : public dimension { -}; -} - -template -struct ChartConcept { -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)); - - /** - * Returns Retraction update of val_ - */ - type retract_ret = C::retract(val_, vec_); - - /** - * 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_); + // Retraction back to manifold + static Class Retract(const Class& origin, const TangentVector& v) { + return origin.retract(v); } - -private: - type val_; - vector vec_; - int dim_; }; -/** - * 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 +} // \ namespace internal -template<> -struct DefaultChart { - typedef double type; - typedef Eigen::Matrix vector; +/// 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); +} - 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; - } -}; +/// Manifold concept +template +class IsManifold { -// Fixed size Eigen::Matrix type +public: -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; + 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; - BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic), - "Internal error: DefaultChart for Dynamic should be handled by template below"); + 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); - 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(); + // make sure Chart methods are defined + v = traits::Local(p, q); + q = traits::Retract(p, v); } -}; -// 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); + TangentVector v; + ManifoldType p, q; +}; - 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 1469e265da..7bcd32b9ff 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 9c273f78c2..c3cbfa341d 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -37,36 +37,36 @@ namespace gtsam { typedef Eigen::MatrixXd Matrix; typedef Eigen::Matrix MatrixRowMajor; -typedef Eigen::Matrix2d Matrix2; -typedef Eigen::Matrix3d Matrix3; -typedef Eigen::Matrix4d Matrix4; -typedef Eigen::Matrix Matrix5; -typedef Eigen::Matrix Matrix6; - -typedef Eigen::Matrix Matrix23; -typedef Eigen::Matrix Matrix24; -typedef Eigen::Matrix Matrix25; -typedef Eigen::Matrix Matrix26; -typedef Eigen::Matrix Matrix27; -typedef Eigen::Matrix Matrix28; -typedef Eigen::Matrix Matrix29; - -typedef Eigen::Matrix Matrix32; -typedef Eigen::Matrix Matrix34; -typedef Eigen::Matrix Matrix35; -typedef Eigen::Matrix Matrix36; -typedef Eigen::Matrix Matrix37; -typedef Eigen::Matrix Matrix38; -typedef Eigen::Matrix Matrix39; +// Create handy typedefs and constants for square-size matrices +// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 +#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX) \ +typedef Eigen::Matrix Matrix##SUFFIX; \ +typedef Eigen::Matrix Matrix1##SUFFIX; \ +typedef Eigen::Matrix Matrix2##SUFFIX; \ +typedef Eigen::Matrix Matrix3##SUFFIX; \ +typedef Eigen::Matrix Matrix4##SUFFIX; \ +typedef Eigen::Matrix Matrix5##SUFFIX; \ +typedef Eigen::Matrix Matrix6##SUFFIX; \ +typedef Eigen::Matrix Matrix7##SUFFIX; \ +typedef Eigen::Matrix Matrix8##SUFFIX; \ +typedef Eigen::Matrix Matrix9##SUFFIX; \ +static const Eigen::MatrixBase::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \ +static const Eigen::MatrixBase::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero(); + +GTSAM_MAKE_TYPEDEFS(1,1); +GTSAM_MAKE_TYPEDEFS(2,2); +GTSAM_MAKE_TYPEDEFS(3,3); +GTSAM_MAKE_TYPEDEFS(4,4); +GTSAM_MAKE_TYPEDEFS(5,5); +GTSAM_MAKE_TYPEDEFS(6,6); +GTSAM_MAKE_TYPEDEFS(7,7); +GTSAM_MAKE_TYPEDEFS(8,8); +GTSAM_MAKE_TYPEDEFS(9,9); // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; typedef Eigen::Block ConstSubMatrix; -// Two very commonly used matrices: -const Matrix3 Z_3x3 = Matrix3::Zero(); -const Matrix3 I_3x3 = Matrix3::Identity(); - // Matlab-like syntax /** @@ -237,7 +237,10 @@ Eigen::Block 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 +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 new file mode 100644 index 0000000000..2ea9d672ec --- /dev/null +++ b/gtsam/base/OptionalJacobian.h @@ -0,0 +1,172 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file OptionalJacobian.h + * @brief Special class for optional Jacobian arguments + * @author Frank Dellaert + * @author Natesh Srinivasan + * @date Nov 28, 2014 + */ + +#pragma once + +#include + +#ifndef OPTIONALJACOBIAN_NOBOOST +#include +#endif + +namespace gtsam { + +/** + * OptionalJacobian is an Eigen::Ref like class that can take be constructed using + * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic + * matrix will be resized. Finally, there is a constructor that takes + * boost::none, the default constructor acts like boost::none, and + * boost::optional is also supported for backwards compatibility. + * Below this class, a dynamic version is also implemented. + */ +template +class OptionalJacobian { + +public: + + /// ::Jacobian size type + typedef Eigen::Matrix Jacobian; + +private: + + 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); + } + +public: + + /// Default constructor acts like boost::none + OptionalJacobian() : + map_(NULL) { + } + + /// Constructor that will usurp data of a fixed-size matrix + OptionalJacobian(Jacobian& fixed) : + map_(NULL) { + usurp(fixed.data()); + } + + /// Constructor that will usurp data of a fixed-size matrix, pointer version + OptionalJacobian(Jacobian* fixedPtr) : + map_(NULL) { + if (fixedPtr) + usurp(fixedPtr->data()); + } + + /// Constructor that will resize a dynamic matrix (unless already correct) + OptionalJacobian(Eigen::MatrixXd& dynamic) : + map_(NULL) { + dynamic.resize(Rows, Cols); // no malloc if correct size + usurp(dynamic.data()); + } + +#ifndef OPTIONALJACOBIAN_NOBOOST + + /// Constructor with boost::none just makes empty + OptionalJacobian(boost::none_t none) : + map_(NULL) { + } + + /// Constructor compatible with old-style derivatives + OptionalJacobian(const boost::optional optional) : + map_(NULL) { + if (optional) { + optional->resize(Rows, Cols); + usurp(optional->data()); + } + } + +#endif + + /// Return true is allocated, false if default constructor was used + operator bool() const { + return map_.data() != NULL; + } + + /// De-reference, like boost optional + Eigen::Map& operator*() { + return map_; + } + + /// TODO: operator->() + 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_!=NULL; + } + + /// 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 a308c50a1e..4790530ab7 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 afc244d6ce..a12f453f45 100644 --- a/gtsam/base/Value.h +++ b/gtsam/base/Value.h @@ -129,9 +129,6 @@ namespace gtsam { template const ValueType& cast() const; - template - const Chart& getChart() const; - /** Virutal destructor */ virtual ~Value() {} @@ -168,7 +165,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 b872aa08be..a9ef8fe105 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/Vector.h b/gtsam/base/Vector.h index 1be8408d23..74cd248a1e 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -34,6 +34,7 @@ namespace gtsam { typedef Eigen::VectorXd Vector; // Commonly used fixed size vectors +typedef Eigen::Matrix Vector1; typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; typedef Eigen::Matrix Vector4; @@ -42,6 +43,7 @@ typedef Eigen::Matrix Vector6; typedef Eigen::Matrix Vector7; typedef Eigen::Matrix Vector8; typedef Eigen::Matrix Vector9; +typedef Eigen::Matrix Vector10; typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h new file mode 100644 index 0000000000..094e6f1622 --- /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 d2f4535210..f63054a5bf 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 0000000000..da872d0063 --- /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/debug.cpp b/gtsam/base/debug.cpp index e50e3af358..6abc98642a 100644 --- a/gtsam/base/debug.cpp +++ b/gtsam/base/debug.cpp @@ -17,9 +17,32 @@ */ #include +#ifdef GTSAM_USE_TBB +#include +#endif namespace gtsam { - GTSAM_EXPORT FastMap > debugFlags; +GTSAM_EXPORT FastMap > debugFlags; + +#ifdef GTSAM_USE_TBB +tbb::mutex debugFlagsMutex; +#endif + +/* ************************************************************************* */ +bool guardedIsDebug(const std::string& s) { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(debugFlagsMutex); +#endif + return gtsam::debugFlags[s]; +} + +/* ************************************************************************* */ +void guardedSetDebug(const std::string& s, const bool v) { +#ifdef GTSAM_USE_TBB + tbb::mutex::scoped_lock lock(debugFlagsMutex); +#endif + gtsam::debugFlags[s] = v; +} } diff --git a/gtsam/base/debug.h b/gtsam/base/debug.h index f416bd8265..e21efcc831 100644 --- a/gtsam/base/debug.h +++ b/gtsam/base/debug.h @@ -43,6 +43,10 @@ namespace gtsam { GTSAM_EXTERN_EXPORT FastMap > debugFlags; + + // Non-guarded use led to crashes, and solved in commit cd35db2 + bool GTSAM_EXPORT guardedIsDebug(const std::string& s); + void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v); } #undef ISDEBUG @@ -50,8 +54,8 @@ namespace gtsam { #ifdef GTSAM_ENABLE_DEBUG -#define ISDEBUG(S) (gtsam::debugFlags[S]) -#define SETDEBUG(S,V) ((void)(gtsam::debugFlags[S] = (V))) +#define ISDEBUG(S) (gtsam::guardedIsDebug(S)) +#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V))) #else diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index b2c163c2d6..a7dc37d55c 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,23 +279,65 @@ inline Matrix numericalDerivative32(Y (*h)(const X1&, const X2&, const X3&), * All classes Y,X1,X2,X3 need dim, expmap, logmap */ template -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); } +/** + * Compute numerical derivative in argument 1 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative41( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + 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, x4), x1, delta); +} + +/** + * Compute numerical derivative in argument 2 of 4-argument function + * @param h ternary function yielding m-vector + * @param x1 n-dimensional first argument value + * @param x2 second argument value + * @param x3 third argument value + * @param x4 fourth argument value + * @param delta increment for numerical derivative + * @return m*n Jacobian computed via central differencing + */ +template +typename internal::FixedSizeMatrix::type numericalDerivative42( + boost::function h, const X1& x1, + const X2& x2, const X3& x3, const X4& x4, double delta = 1e-5) { + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument Y must be a manifold type."); + BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::structure_category>::value), + "Template argument X1 must be a manifold type."); + return numericalDerivative11(boost::bind(h, x1, _1, x3, x4), x2, delta); +} + /** * Compute numerical Hessian matrix. Requires a single-argument Lie->scalar * function. This is implemented simply as the derivative of the gradient. @@ -301,19 +347,20 @@ inline Matrix numericalDerivative33(Y (*h)(const X1&, const X2&, const X3&), * @return n*n Hessian matrix computed via central differencing */ template -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 +374,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 +386,10 @@ class G_x1 { }; 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 +397,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 +420,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 +441,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 +452,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 +466,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 +475,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 +489,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 +498,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 +512,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 +521,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 +530,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 +539,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 +549,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 +557,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 +565,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 0000000000..791d5c04d2 --- /dev/null +++ b/gtsam/base/testLie.h @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file chartTesting.h + * @brief + * @date November, 2014 + * @author Paul Furgale + */ + +#pragma once + +#include +#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; + typedef OptionalJacobian OJ; + + // Inverse + OJ none; + EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t1, none),H1)); + + EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); + EXPECT(assert_equal(numericalDerivative21(T::Inverse, t2, none),H1)); + + // Compose + EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative41(T::Compose, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Compose, t1, t2, none, none), H2)); + + // Between + EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative41(T::Between, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Between, t1, t2, none, none), 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; + typedef typename G::TangentVector V; + typedef OptionalJacobian OJ; + + // Retract + OJ none; + V w12 = T::Local(t1, t2); + EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); + EXPECT(assert_equal(numericalDerivative41(T::Retract, t1, w12, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Retract, t1, w12, none, none), H2)); + + // Local + EXPECT(assert_equal(w12, t1.localCoordinates(t2, H1, H2))); + EXPECT(assert_equal(numericalDerivative41(T::Local, t1, t2, none, none), H1)); + EXPECT(assert_equal(numericalDerivative42(T::Local, t1, t2, none, none), H2)); +} +} // namespace gtsam + +#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \ + { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); } + +#define CHECK_CHART_DERIVATIVES(t1,t2) \ + { gtsam::testChartDerivatives(result_, name_, t1, t2); } diff --git a/gtsam/base/tests/testLieMatrix.cpp b/gtsam/base/tests/testLieMatrix.cpp index 53dd6d4d5c..7cc649e66d 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 946a342fc6..060087c2ab 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 c1dface2ea..81e03c63c6 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 5b36d2b02e..13d32794e8 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 d3a2b929f9..1cbf71e1f2 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 new file mode 100644 index 0000000000..331fb49ebf --- /dev/null +++ b/gtsam/base/tests/testOptionalJacobian.cpp @@ -0,0 +1,147 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testOptionalJacobian.cpp + * @brief Unit test for OptionalJacobian + * @author Frank Dellaert + * @date Nov 28, 2014 + **/ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +//****************************************************************************** +TEST( OptionalJacobian, Constructors ) { + Matrix23 fixed; + + OptionalJacobian<2, 3> H1; + EXPECT(!H1); + + OptionalJacobian<2, 3> H2(fixed); + EXPECT(H2); + + OptionalJacobian<2, 3> H3(&fixed); + EXPECT(H3); + + Matrix dynamic; + OptionalJacobian<2, 3> H4(dynamic); + EXPECT(H4); + + OptionalJacobian<2, 3> H5(boost::none); + EXPECT(!H5); + + boost::optional optional(dynamic); + OptionalJacobian<2, 3> H6(optional); + EXPECT(H6); + + OptionalJacobian<-1, -1> H7; + EXPECT(!H7); + + OptionalJacobian<-1, -1> H8(dynamic); + EXPECT(H8); + + OptionalJacobian<-1, -1> H9(boost::none); + EXPECT(!H9); + + OptionalJacobian<-1, -1> H10(optional); + EXPECT(H10); +} + +//****************************************************************************** +void test(OptionalJacobian<2, 3> H = boost::none) { + if (H) + *H = Matrix23::Zero(); +} + +void testPtr(Matrix23* H = NULL) { + if (H) + *H = Matrix23::Zero(); +} + +TEST( OptionalJacobian, Fixed) { + + Matrix expected = Matrix23::Zero(); + + // Default argument does nothing + test(); + + // Fixed size, no copy + Matrix23 fixed1; + fixed1.setOnes(); + test(fixed1); + EXPECT(assert_equal(expected,fixed1)); + + // Fixed size, no copy, pointer style + Matrix23 fixed2; + fixed2.setOnes(); + test(&fixed2); + EXPECT(assert_equal(expected,fixed2)); + + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3, 5); + dynamic1.setOnes(); + test(dynamic1); + EXPECT(assert_equal(expected,dynamic1)); + + // Dynamic right size + Matrix dynamic2(2, 5); + dynamic2.setOnes(); + test(dynamic2); + EXPECT(assert_equal(expected, dynamic2)); +} + +//****************************************************************************** +void test2(OptionalJacobian<-1,-1> H = boost::none) { + if (H) + *H = Matrix23::Zero(); // resizes +} + +TEST( OptionalJacobian, Dynamic) { + + Matrix expected = Matrix23::Zero(); + + // Default argument does nothing + test2(); + + // Empty is no longer a sign we don't want a matrix, we want it resized + Matrix dynamic0; + test2(dynamic0); + EXPECT(assert_equal(expected,dynamic0)); + + // Dynamic wrong size + Matrix dynamic1(3, 5); + dynamic1.setOnes(); + test2(dynamic1); + EXPECT(assert_equal(expected,dynamic1)); + + // Dynamic right size + Matrix dynamic2(2, 5); + dynamic2.setOnes(); + test2(dynamic2); + EXPECT(assert_equal(expected, dynamic2)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/base/tests/testVector.cpp b/gtsam/base/tests/testVector.cpp index 460ff6cd6d..00e40039bf 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 dc74661994..f7253ec242 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 8cb2db182d..de5ec80425 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 1ba97444ff..88c3e56204 100644 --- a/gtsam/discrete/DiscreteConditional.h +++ b/gtsam/discrete/DiscreteConditional.h @@ -130,6 +130,9 @@ class GTSAM_EXPORT DiscreteConditional: public DecisionTreeFactor, }; // 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 8351b310bc..e24dfdf2a8 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { @@ -101,4 +102,8 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { }; // 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 27eb722d99..c5b11adf18 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -144,7 +144,10 @@ public EliminateableFactorGraph { // // /** 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 51475e07d2..978781d638 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 f8bcb45c2a..970a18b42e 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 1dfd56eec2..05223c67a7 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.cpp b/gtsam/geometry/Cal3Bundler.cpp index 8ff728d265..368ae6c986 100644 --- a/gtsam/geometry/Cal3Bundler.cpp +++ b/gtsam/geometry/Cal3Bundler.cpp @@ -34,20 +34,22 @@ Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : } /* ************************************************************************* */ -Matrix Cal3Bundler::K() const { +Matrix3 Cal3Bundler::K() const { Matrix3 K; K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1; return K; } /* ************************************************************************* */ -Vector Cal3Bundler::k() const { - return (Vector(4) << k1_, k2_, 0, 0).finished(); +Vector4 Cal3Bundler::k() const { + Vector4 rvalue_; + rvalue_ << k1_, k2_, 0, 0; + return rvalue_; } /* ************************************************************************* */ Vector3 Cal3Bundler::vector() const { - return (Vector(3) << f_, k1_, k2_).finished(); + return Vector3(f_, k1_, k2_); } /* ************************************************************************* */ @@ -64,48 +66,9 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const { return true; } -/* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p) const { - // r = x^2 + y^2; - // g = (1 + k(1)*r + k(2)*r^2); - // pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y(); - const double r = x * x + y * y; - const double g = 1. + (k1_ + k2_ * r) * r; - const double u = g * x, v = g * y; - return Point2(u0_ + f_ * u, v0_ + f_ * v); -} - -/* ************************************************************************* */ -Point2 Cal3Bundler::uncalibrate(const Point2& p, // - boost::optional Dcal, boost::optional Dp) const { - // r = x^2 + y^2; - // g = (1 + k(1)*r + k(2)*r^2); - // pi(:,i) = g * pn(:,i) - const double x = p.x(), y = p.y(); - const double r = x * x + y * y; - const double g = 1. + (k1_ + k2_ * r) * r; - const double u = g * x, v = g * y; - - // Derivatives make use of intermediate variables above - if (Dcal) { - double rx = r * x, ry = r * y; - *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; - } - - if (Dp) { - const double a = 2. * (k1_ + 2. * k2_ * r); - const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; - *Dp << g + axx, axy, axy, g + ayy; - *Dp *= f_; - } - - return Point2(u0_ + f_ * u, v0_ + f_ * v); -} - /* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, // - boost::optional Dcal, boost::optional Dp) const { + OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const { // r = x^2 + y^2; // g = (1 + k(1)*r + k(2)*r^2); // pi(:,i) = g * pn(:,i) @@ -117,14 +80,12 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, // // Derivatives make use of intermediate variables above if (Dcal) { double rx = r * x, ry = r * y; - Dcal->resize(2, 3); *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry; } if (Dp) { const double a = 2. * (k1_ + 2. * k2_ * r); const double axx = a * x * x, axy = a * x * y, ayy = a * y * y; - Dp->resize(2,2); *Dp << g + axx, axy, axy, g + ayy; *Dp *= f_; } @@ -161,24 +122,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const { } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const { - Matrix Dp; +Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const { + Matrix2 Dp; uncalibrate(p, boost::none, Dp); return Dp; } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_calibration(const Point2& p) const { - Matrix Dcal; +Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const { + Matrix23 Dcal; uncalibrate(p, Dcal, boost::none); return Dcal; } /* ************************************************************************* */ -Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { - Matrix Dcal, Dp; +Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const { + Matrix23 Dcal; + Matrix2 Dp; uncalibrate(p, Dcal, Dp); - Matrix H(2, 5); + Matrix25 H; H << Dp, Dcal; return H; } diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 4c77fdf234..ce9f94c480 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -37,6 +37,8 @@ class GTSAM_EXPORT Cal3Bundler { public: + enum { dimension = 3 }; + /// @name Standard Constructors /// @{ @@ -69,8 +71,8 @@ class GTSAM_EXPORT Cal3Bundler { /// @name Standard Interface /// @{ - Matrix K() const; ///< Standard 3*3 calibration matrix - Vector k() const; ///< Radial distortion parameters (4 of them, 2 0) + Matrix3 K() const; ///< Standard 3*3 calibration matrix + Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0) Vector3 vector() const; @@ -106,43 +108,27 @@ class GTSAM_EXPORT Cal3Bundler { /** - * convert intrinsic coordinates xy to image coordinates uv - * @param p point in intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p) const; - - /** - * Version of uncalibrate with fixed derivatives + * @brief: convert intrinsic coordinates xy to image coordinates uv + * Version of uncalibrate with derivatives * @param p point in intrinsic coordinates * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; - - /** - * Version of uncalibrate with dynamic derivatives - * @param p point in intrinsic coordinates - * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none, + OptionalJacobian<2, 2> Dp = boost::none) const; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_intrinsic(const Point2& p) const; + Matrix2 D2d_intrinsic(const Point2& p) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_calibration(const Point2& p) const; + Matrix23 D2d_calibration(const Point2& p) const; /// @deprecated might be removed in next release, use uncalibrate - Matrix D2d_intrinsic_calibration(const Point2& p) const; + Matrix25 D2d_intrinsic_calibration(const Point2& p) const; /// @} /// @name Manifold @@ -185,24 +171,7 @@ class GTSAM_EXPORT Cal3Bundler { }; -// 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 fb338431a1..0c77eebbc3 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 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base { }; -// 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/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 1105fecfb4..12060c12d2 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -28,18 +28,22 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v): fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} /* ************************************************************************* */ -Matrix Cal3DS2_Base::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished(); +Matrix3 Cal3DS2_Base::K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; } /* ************************************************************************* */ -Vector Cal3DS2_Base::vector() const { - return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished(); +Vector9 Cal3DS2_Base::vector() const { + Vector9 v; + v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_; + return v; } /* ************************************************************************* */ void Cal3DS2_Base::print(const std::string& s_) const { - gtsam::print(K(), s_ + ".K"); + gtsam::print((Matrix)K(), s_ + ".K"); gtsam::print(Vector(k()), s_ + ".k"); } @@ -91,8 +95,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, /* ************************************************************************* */ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { + OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -126,26 +129,6 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } -/* ************************************************************************* */ -Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { - - if (H1 || H2) { - Matrix29 D1; - Matrix2 D2; - Point2 pu = uncalibrate(p, D1, D2); - if (H1) - *H1 = D1; - if (H2) - *H2 = D2; - return pu; - - } else { - return uncalibrate(p); - } -} - /* ************************************************************************* */ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { // Use the following fixed point iteration to invert the radial distortion. @@ -177,7 +160,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { } /* ************************************************************************* */ -Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { +Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; const double rr = xx + yy; const double r4 = rr * rr; @@ -188,7 +171,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { } /* ************************************************************************* */ -Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const { +Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const { const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; const double rr = xx + yy; const double r4 = rr * rr; diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 61c01e349c..37c156743d 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -45,9 +45,9 @@ class GTSAM_EXPORT Cal3DS2_Base { double p1_, p2_ ; // tangential distortion public: - Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } - Vector vector() const ; + Matrix3 K() const ; + Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); } + Vector9 vector() const ; /// @name Standard Constructors /// @{ @@ -113,23 +113,18 @@ class GTSAM_EXPORT Cal3DS2_Base { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in (distorted) image coordinates */ - - Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; - Point2 uncalibrate(const Point2& p, - boost::optional Dcal, - boost::optional Dp) const ; + OptionalJacobian<2,9> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; /// Derivative of uncalibrate wrpt intrinsic coordinates - Matrix D2d_intrinsic(const Point2& p) const ; + Matrix2 D2d_intrinsic(const Point2& p) const ; /// Derivative of uncalibrate wrpt the calibration parameters - Matrix D2d_calibration(const Point2& p) const ; + Matrix29 D2d_calibration(const Point2& p) const ; private: diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 6bc4c4bb31..8b7c1abf46 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -29,8 +29,10 @@ Cal3Unified::Cal3Unified(const Vector &v): Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} /* ************************************************************************* */ -Vector Cal3Unified::vector() const { - return (Vector(10) << Base::vector(), xi_).finished(); +Vector10 Cal3Unified::vector() const { + Vector10 v; + v << Base::vector(), xi_; + return v; } /* ************************************************************************* */ @@ -52,8 +54,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { /* ************************************************************************* */ // todo: make a fixed sized jacobian version of this Point2 Cal3Unified::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { + OptionalJacobian<2,10> H1, + OptionalJacobian<2,2> H2) const { // this part of code is modified from Cal3DS2, // since the second part of this model (after project to normalized plane) @@ -81,10 +83,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, Vector2 DU; DU << -xs * sqrt_nx * xi_sqrt_nx2, // -ys * sqrt_nx * xi_sqrt_nx2; - - H1->resize(2,10); - H1->block<2,9>(0,0) = H1base; - H1->block<2,1>(0,9) = H2base * DU; + *H1 << H1base, H2base * DU; } // Inlined derivative for points @@ -96,7 +95,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, // mid, (sqrt_nx + xi*(xs*xs + 1)) * denom; - *H2 = H2base * DU; + *H2 << H2base * DU; } return puncalib; @@ -136,7 +135,7 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const { } /* ************************************************************************* */ -Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { +Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const { return T2.vector() - vector(); } diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index f65b277801..d0e0f38913 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -50,8 +50,9 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { double xi_; // mirror parameter public: + enum { dimension = 10 }; - Vector vector() const ; + Vector10 vector() const ; /// @name Standard Constructors /// @{ @@ -96,8 +97,8 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { * @return point in image coordinates */ Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; + OptionalJacobian<2,10> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const ; /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, const double tol=1e-5) const; @@ -116,7 +117,7 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { Cal3Unified retract(const Vector& d) const ; /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3Unified& T2) const ; + Vector10 localCoordinates(const Cal3Unified& T2) const ; /// Return dimensions of calibration manifold object virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) @@ -138,23 +139,8 @@ class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base { }; -// 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.cpp b/gtsam/geometry/Cal3_S2.cpp index aece1ded1e..3ec29bbd23 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -53,7 +53,7 @@ Cal3_S2::Cal3_S2(const std::string &path) : /* ************************************************************************* */ void Cal3_S2::print(const std::string& s) const { - gtsam::print(matrix(), s); + gtsam::print((Matrix)matrix(), s); } /* ************************************************************************* */ @@ -72,32 +72,13 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const { +Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) const { const double x = p.x(), y = p.y(); - if (Dcal) { - Dcal->resize(2,5); + if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - } - if (Dp) { - Dp->resize(2,2); + if (Dp) *Dp << fx_, s_, 0.0, fy_; - } - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); -} - -/* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const { - const double x = p.x(), y = p.y(); - if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; - if (Dp) *Dp << fx_, s_, 0.0, fy_; - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); -} - -/* ************************************************************************* */ -Point2 Cal3_S2::uncalibrate(const Point2& p) const { - const double x = p.x(), y = p.y(); return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 6317d251d6..0c5386822e 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -36,7 +36,7 @@ class GTSAM_EXPORT Cal3_S2 { double fx_, fy_, s_, u0_, v0_; public: - + enum { dimension = 5 }; typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object /// @name Standard Constructors @@ -117,37 +117,33 @@ class GTSAM_EXPORT Cal3_S2 { } /// vectorized form (column-wise) - Vector vector() const { - double r[] = { fx_, fy_, s_, u0_, v0_ }; - Vector v(5); - std::copy(r, r + 5, v.data()); + Vector5 vector() const { + Vector5 v; + v << fx_, fy_, s_, u0_, v0_; return v; } /// return calibration matrix K - Matrix K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished(); + Matrix3 K() const { + Matrix3 K; + K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; + return K; } /** @deprecated The following function has been deprecated, use K above */ - Matrix matrix() const { + Matrix3 matrix() const { return K(); } /// return inverted calibration matrix inv(K) - Matrix matrix_inverse() const { + Matrix3 matrix_inverse() const { const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; - return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, - 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished(); + Matrix3 K_inverse; + K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, + 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0; + return K_inverse; } - /** - * convert intrinsic coordinates xy to image coordinates uv - * @param p point in intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p) const; - /** * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves * @param p point in intrinsic coordinates @@ -155,18 +151,8 @@ class GTSAM_EXPORT Cal3_S2 { * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; - - /** - * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves - * @param p point in intrinsic coordinates - * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters - * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates - */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal, - boost::optional Dp) const; + Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + OptionalJacobian<2,2> Dp = boost::none) const; /** * convert image coordinates uv to intrinsic coordinates xy @@ -184,10 +170,10 @@ class GTSAM_EXPORT Cal3_S2 { /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) inline Cal3_S2 between(const Cal3_S2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(5); - if(H2) *H2 = eye(5); + OptionalJacobian<5,5> H1=boost::none, + OptionalJacobian<5,5> H2=boost::none) const { + if(H1) *H1 = -I_5x5; + if(H2) *H2 = I_5x5; return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_); } @@ -212,7 +198,7 @@ class GTSAM_EXPORT Cal3_S2 { } /// Unretraction for the calibration - Vector localCoordinates(const Cal3_S2& T2) const { + Vector5 localCoordinates(const Cal3_S2& T2) const { return T2.vector() - vector(); } @@ -237,22 +223,7 @@ class GTSAM_EXPORT Cal3_S2 { }; -// 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 b471535474..3585fb1565 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 @@ -103,6 +104,38 @@ namespace gtsam { /// return baseline inline double baseline() const { return b_; } + /// vectorized form (column-wise) + Vector6 vector() const { + Vector6 v; + v << K_.vector(), b_; + return v; + } + + /// @} + /// @name Manifold + /// @{ + + /// return DOF, dimensionality of tangent space + inline size_t dim() const { + return 6; + } + + /// return DOF, dimensionality of tangent space + static size_t Dim() { + return 6; + } + + /// Given 6-dim tangent vector, create new calibration + inline Cal3_S2Stereo retract(const Vector& d) const { + return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5)); + } + + /// Unretraction for the calibration + Vector6 localCoordinates(const Cal3_S2Stereo& T2) const { + return T2.vector() - vector(); + } + + /// @} /// @name Advanced Interface /// @{ @@ -119,4 +152,10 @@ namespace gtsam { /// @} }; + + // Define GTSAM traits + template<> + struct traits : public internal::Manifold { + }; + } // \ namespace gtsam diff --git a/gtsam/geometry/CalibratedCamera.cpp b/gtsam/geometry/CalibratedCamera.cpp index 392a53858b..1f5f1f8a5c 100644 --- a/gtsam/geometry/CalibratedCamera.cpp +++ b/gtsam/geometry/CalibratedCamera.cpp @@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) : /* ************************************************************************* */ Point2 CalibratedCamera::project_to_camera(const Point3& P, - boost::optional H1) { + OptionalJacobian<2,3> H1) { if (H1) { double d = 1.0 / P.z(), d2 = d * d; - *H1 = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2).finished(); + *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2; } return Point2(P.x() / P.z(), P.y() / P.z()); } @@ -59,10 +59,12 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { /* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, - boost::optional Dpose, boost::optional Dpoint) const { + OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const { #ifdef CALIBRATEDCAMERA_CHAIN_RULE - Point3 q = pose_.transform_to(point, Dpose, Dpoint); + Matrix36 Dpose_; + Matrix3 Dpoint_; + Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0); #else Point3 q = pose_.transform_to(point); #endif @@ -75,23 +77,26 @@ Point2 CalibratedCamera::project(const Point3& point, if (Dpose || Dpoint) { #ifdef CALIBRATEDCAMERA_CHAIN_RULE // just implement chain rule - Matrix H; - project_to_camera(q,H); - if (Dpose) *Dpose = H * (*Dpose); - if (Dpoint) *Dpoint = H * (*Dpoint); + if(Dpose && Dpoint) { + Matrix23 H; + project_to_camera(q,H); + if (Dpose) *Dpose = H * (*Dpose_); + if (Dpoint) *Dpoint = H * (*Dpoint_); + } #else // optimized version, see CalibratedCamera.nb const double z = q.z(), d = 1.0 / z; const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v; if (Dpose) - *Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), - -uv, -u, 0., -d, d * v).finished(); + *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), + -uv, -u, 0., -d, d * v; if (Dpoint) { - const Matrix R(pose_.rotation().matrix()); - *Dpoint = d - * (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), + const Matrix3 R(pose_.rotation().matrix()); + Matrix23 Dpoint_; + Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2), - R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished(); + R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); + *Dpoint = d * Dpoint_; } #endif } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index c669410917..cda01b600d 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -18,9 +18,8 @@ #pragma once -#include -#include #include +#include namespace gtsam { @@ -45,6 +44,8 @@ class GTSAM_EXPORT CalibratedCamera { public: + enum { dimension = 6 }; + /// @name Standard Constructors /// @{ @@ -88,26 +89,6 @@ class GTSAM_EXPORT CalibratedCamera { return pose_; } - /// compose the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera compose(const CalibratedCamera &c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - return CalibratedCamera(pose_.compose(c.pose(), H1, H2)); - } - - /// between the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera between(const CalibratedCamera& c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - return CalibratedCamera(pose_.between(c.pose(), H1, H2)); - } - - /// invert the camera pose: TODO Frank says this might not make sense - inline const CalibratedCamera inverse(boost::optional H1 = - boost::none) const { - return CalibratedCamera(pose_.inverse(H1)); - } - /** * Create a level camera at the given 2D pose and height * @param pose2 specifies the location and viewing direction @@ -152,8 +133,8 @@ class GTSAM_EXPORT CalibratedCamera { * @return the intrinsic coordinates of the projected point */ Point2 project(const Point3& point, - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none) const; + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const; /** * projects a 3-dimensional point in camera coordinates into the @@ -161,7 +142,7 @@ class GTSAM_EXPORT CalibratedCamera { * With optional 2by3 derivative */ static Point2 project_to_camera(const Point3& cameraPoint, - boost::optional H1 = boost::none); + OptionalJacobian<2, 3> H1 = boost::none); /** * backproject a 2-dimensional point to a 3-dimension point @@ -175,8 +156,8 @@ class GTSAM_EXPORT CalibratedCamera { * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Point3& point, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const { return pose_.range(point, H1, H2); } @@ -187,8 +168,8 @@ class GTSAM_EXPORT CalibratedCamera { * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Pose3& pose, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 6> H2 = boost::none) const { return pose_.range(pose, H1, H2); } @@ -199,8 +180,8 @@ class GTSAM_EXPORT CalibratedCamera { * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const CalibratedCamera& camera, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { + double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 = + boost::none, OptionalJacobian<1, 6> H2 = boost::none) const { return pose_.range(camera.pose_, H1, H2); } @@ -220,22 +201,8 @@ class GTSAM_EXPORT CalibratedCamera { /// @} }; -// 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::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 f1bb947a28..7242459a6d 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 0000000000..2c883707fe --- /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.cpp b/gtsam/geometry/EssentialMatrix.cpp index e65e5d0975..062178fea5 100644 --- a/gtsam/geometry/EssentialMatrix.cpp +++ b/gtsam/geometry/EssentialMatrix.cpp @@ -14,7 +14,7 @@ namespace gtsam { /* ************************************************************************* */ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, - boost::optional H) { + OptionalJacobian<5, 6> H) { const Rot3& _1R2_ = _1P2_.rotation(); const Point3& _1T2_ = _1P2_.translation(); if (!H) { @@ -25,13 +25,10 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_, // Calculate the 5*6 Jacobian H = D_E_1P2 // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation // First get 2*3 derivative from Unit3::FromPoint3 - Matrix D_direction_1T2; + Matrix23 D_direction_1T2; Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2); - H->resize(5, 6); - H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left - H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left - H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right - H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right + *H << I_3x3, Z_3x3, // + Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix(); return EssentialMatrix(_1R2_, direction); } } @@ -54,23 +51,26 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const { } /* ************************************************************************* */ -Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { - return (Vector(5) << - aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished(); +Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const { + Vector5 v; + v << aRb_.localCoordinates(other.aRb_), + aTb_.localCoordinates(other.aTb_); + return v; } /* ************************************************************************* */ -Point3 EssentialMatrix::transform_to(const Point3& p, - boost::optional DE, boost::optional Dpoint) const { +Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE, + OptionalJacobian<3, 3> Dpoint) const { Pose3 pose(aRb_, aTb_.point3()); - Point3 q = pose.transform_to(p, DE, Dpoint); + Matrix36 DE_; + Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint); if (DE) { // DE returned by pose.transform_to is 3*6, but we need it to be 3*5 // The last 3 columns are derivative with respect to change in translation // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis() // Duy made an educated guess that this needs to be rotated to the local frame - Matrix H(3, 5); - H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis(); + Matrix35 H; + H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis(); *DE = H; } return q; @@ -78,7 +78,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p, /* ************************************************************************* */ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, - boost::optional HE, boost::optional HR) const { + OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const { // The rotation must be conjugated to act in the camera frame Rot3 c1Rc2 = aRb_.conjugate(cRb); @@ -89,18 +89,16 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, return EssentialMatrix(c1Rc2, c1Tc2); } else { // Calculate derivatives - Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2 + Matrix23 D_c1Tc2_cRb; // 2*3 + Matrix2 D_c1Tc2_aTb; // 2*2 Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb); - if (HE) { - *HE = zeros(5, 5); - HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2 - HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2) - } + if (HE) + *HE << cRb.matrix(), Matrix32::Zero(), // + Matrix23::Zero(), D_c1Tc2_aTb; if (HR) { throw runtime_error( "EssentialMatrix::rotate: derivative HR not implemented yet"); /* - HR->resize(5, 3); HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ? HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ? */ @@ -110,13 +108,12 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb, } /* ************************************************************************* */ -double EssentialMatrix::error(const Vector& vA, const Vector& vB, // - boost::optional H) const { +double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1, 5> H) const { if (H) { - H->resize(1, 5); // See math.lyx - Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB); - Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) + Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB); + Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB) * aTb_.basis(); *H << HR, HD; } diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index fd5f45160a..c9e7020783 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -30,9 +30,11 @@ class GTSAM_EXPORT EssentialMatrix { public: + enum { dimension = 5 }; + /// Static function to convert Point2 to homogeneous coordinates - static Vector Homogeneous(const Point2& p) { - return (Vector(3) << p.x(), p.y(), 1).finished(); + static Vector3 Homogeneous(const Point2& p) { + return Vector3(p.x(), p.y(), 1); } /// @name Constructors and named constructors @@ -50,7 +52,7 @@ class GTSAM_EXPORT EssentialMatrix { /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale) static EssentialMatrix FromPose3(const Pose3& _1P2_, - boost::optional H = boost::none); + OptionalJacobian<5, 6> H = boost::none); /// Random, using Rot3::Random and Unit3::Random template @@ -84,15 +86,15 @@ class GTSAM_EXPORT EssentialMatrix { } /// Return the dimensionality of the tangent space - virtual size_t dim() const { + size_t dim() const { return 5; } /// Retract delta to manifold - virtual EssentialMatrix retract(const Vector& xi) const; + EssentialMatrix retract(const Vector& xi) const; /// Compute the coordinates in the tangent space - virtual Vector localCoordinates(const EssentialMatrix& other) const; + Vector5 localCoordinates(const EssentialMatrix& other) const; /// @} @@ -132,16 +134,16 @@ class GTSAM_EXPORT EssentialMatrix { * @return point in pose coordinates */ Point3 transform_to(const Point3& p, - boost::optional DE = boost::none, - boost::optional Dpoint = boost::none) const; + OptionalJacobian<3,5> DE = boost::none, + OptionalJacobian<3,3> Dpoint = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C * @param cRb rotation from body frame to camera frame * @param E essential matrix E in camera frame C */ - EssentialMatrix rotate(const Rot3& cRb, boost::optional HE = - boost::none, boost::optional HR = boost::none) const; + EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE = + boost::none, OptionalJacobian<5, 3> HR = boost::none) const; /** * Given essential matrix E in camera frame B, convert to body frame C @@ -153,8 +155,8 @@ class GTSAM_EXPORT EssentialMatrix { } /// epipolar error, algebraic - double error(const Vector& vA, const Vector& vB, // - boost::optional H = boost::none) const; + double error(const Vector3& vA, const Vector3& vB, // + OptionalJacobian<1,5> H = boost::none) const; /// @} @@ -196,23 +198,8 @@ class GTSAM_EXPORT EssentialMatrix { }; -// 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 c5174ae655..b409c956d4 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -18,16 +18,9 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include #include +#include +#include namespace gtsam { @@ -38,14 +31,20 @@ namespace gtsam { */ template class PinholeCamera { + private: Pose3 pose_; Calibration K_; - static const int DimK = traits::dimension::value; + 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 /// @{ @@ -114,9 +113,9 @@ class PinholeCamera { /// @{ explicit PinholeCamera(const Vector &v) { - pose_ = Pose3::Expmap(v.head(Pose3::Dim())); - if (v.size() > Pose3::Dim()) { - K_ = Calibration(v.tail(Calibration::Dim())); + pose_ = Pose3::Expmap(v.head(6)); + if (v.size() > 6) { + K_ = Calibration(v.tail(DimK)); } } @@ -168,98 +167,42 @@ class PinholeCamera { } /// @} - /// @name Group ?? Frank says this might not make sense + /// @name Manifold /// @{ - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const PinholeCamera &c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - if (H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// between two cameras: TODO Frank says this might not make sense - inline const PinholeCamera between(const PinholeCamera& c, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - PinholeCamera result(pose_.between(c.pose(), H1, H2), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - if (H2) { - H2->conservativeResize(Dim(), Dim()); - H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; - } - - /// inverse camera: TODO Frank says this might not make sense - inline const PinholeCamera inverse( - boost::optional H1 = boost::none) const { - PinholeCamera result(pose_.inverse(H1), K_); - if (H1) { - H1->conservativeResize(Dim(), Dim()); - H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(), - Calibration::Dim()); - H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); - } - return result; + /// Manifold dimension + inline size_t dim() const { + return dimension; } - /// compose two cameras: TODO Frank says this might not make sense - inline const PinholeCamera compose(const Pose3 &c) const { - return PinholeCamera(pose_.compose(c), K_); + /// Manifold dimension + inline static size_t Dim() { + return dimension; } - /// @} - /// @name Manifold - /// @{ + typedef Eigen::Matrix VectorK6; /// move a cameras according to d PinholeCamera retract(const Vector& d) const { - if ((size_t) d.size() == pose_.dim()) + if ((size_t) d.size() == 6) return PinholeCamera(pose().retract(d), calibration()); else - return PinholeCamera(pose().retract(d.head(pose().dim())), + return PinholeCamera(pose().retract(d.head(6)), calibration().retract(d.tail(calibration().dim()))); } - typedef Eigen::Matrix VectorK6; - /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { - VectorK6 d; // TODO: why does d.head<6>() not compile?? + VectorK6 d; + // TODO: why does d.head<6>() not compile?? d.head(6) = pose().localCoordinates(T2.pose()); d.tail(DimK) = calibration().localCoordinates(T2.calibration()); return d; } - /// Manifold dimension - inline size_t dim() const { - return pose_.dim() + K_.dim(); - } - - /// Manifold dimension - inline static size_t Dim() { - return Pose3::Dim() + Calibration::Dim(); + /// for Canonical + static PinholeCamera identity() { + return PinholeCamera(); // assumes that the default constructor is valid } /// @} @@ -272,8 +215,8 @@ class PinholeCamera { * @param P A point in camera coordinates * @param Dpoint is the 2*3 Jacobian w.r.t. P */ - inline static Point2 project_to_camera(const Point3& P, - boost::optional Dpoint = boost::none) { + static Point2 project_to_camera(const Point3& P, // + OptionalJacobian<2, 3> Dpoint = boost::none) { #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION if (P.z() <= 0) throw CheiralityException(); @@ -292,21 +235,7 @@ class PinholeCamera { return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); } - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - */ - inline Point2 project(const Point3& pw) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - return K_.uncalibrate(pn); - } - - typedef Eigen::Matrix Matrix2K; + typedef Eigen::Matrix Matrix2K; /** project a point from world coordinate to the image * @param pw is a point in world coordinates @@ -314,11 +243,9 @@ class PinholeCamera { * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - inline Point2 project( - const Point3& pw, // - boost::optional Dpose, - boost::optional Dpoint, - boost::optional Dcal) const { + inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = + boost::none, OptionalJacobian<2, 3> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { // Transform to camera coordinates and check cheirality const Point3 pc = pose_.transform_to(pw); @@ -340,46 +267,7 @@ class PinholeCamera { calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); return pi; } else - return K_.uncalibrate(pn, Dcal, boost::none); - } - - /** project a point from world coordinate to the image - * @param pw is a point in world coordinates - * @param Dpose is the Jacobian w.r.t. pose3 - * @param Dpoint is the Jacobian w.r.t. point3 - * @param Dcal is the Jacobian w.r.t. calibration - */ - inline Point2 project( - const Point3& pw, // - boost::optional Dpose, - boost::optional Dpoint, - boost::optional Dcal) const { - - // Transform to camera coordinates and check cheirality - const Point3 pc = pose_.transform_to(pw); - - // Project to normalized image coordinates - const Point2 pn = project_to_camera(pc); - - if (Dpose || Dpoint) { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix Dpi_pn(2, 2); - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - // chain the Jacobian matrices - if (Dpose) { - Dpose->resize(2, 6); - calculateDpose(pn, d, Dpi_pn, *Dpose); - } - if (Dpoint) { - Dpoint->resize(2, 3); - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } else - return K_.uncalibrate(pn, Dcal, boost::none); + return K_.uncalibrate(pn, Dcal); } /** project a point at infinity from world coordinate to the image @@ -388,11 +276,10 @@ class PinholeCamera { * @param Dpoint is the Jacobian w.r.t. point3 * @param Dcal is the Jacobian w.r.t. calibration */ - inline Point2 projectPointAtInfinity( - const Point3& pw, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none, - boost::optional Dcal = boost::none) const { + inline Point2 projectPointAtInfinity(const Point3& pw, + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 2> Dpoint = boost::none, + OptionalJacobian<2, DimK> Dcal = boost::none) const { if (!Dpose && !Dpoint && !Dcal) { const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) @@ -401,40 +288,30 @@ class PinholeCamera { } // world to camera coordinate - Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */; + Matrix3 Dpc_rot, Dpc_point; const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point); - Matrix Dpc_pose = Matrix::Zero(3, 6); - Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; + Matrix36 Dpc_pose; + Dpc_pose.setZero(); + Dpc_pose.leftCols<3>() = Dpc_rot; // camera to normalized image coordinate Matrix23 Dpn_pc; // 2*3 const Point2 pn = project_to_camera(pc, Dpn_pc); // uncalibration - Matrix Dpi_pn; // 2*2 + Matrix2 Dpi_pn; // 2*2 const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); // chain the Jacobian matrices - const Matrix Dpi_pc = Dpi_pn * Dpn_pc; + const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; if (Dpose) *Dpose = Dpi_pc * Dpc_pose; if (Dpoint) - *Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only) + *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) return pi; } - typedef Eigen::Matrix Matrix2K6; - - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - */ - Point2 project2(const Point3& pw) const { - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - return K_.uncalibrate(pn); - } - /** project a point from world coordinate to the image, fixed Jacobians * @param pw is a point in the world coordinate * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] @@ -442,8 +319,8 @@ class PinholeCamera { */ Point2 project2( const Point3& pw, // - boost::optional Dcamera, - boost::optional Dpoint) const { + OptionalJacobian<2, dimension> Dcamera = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); const Point2 pn = project_to_camera(pc); @@ -459,8 +336,8 @@ class PinholeCamera { const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); if (Dcamera) { // TODO why does leftCols<6>() not compile ?? - calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6)); - Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib + calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); + (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib } if (Dpoint) { calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); @@ -469,40 +346,6 @@ class PinholeCamera { } } - /** project a point from world coordinate to the image - * @param pw is a point in the world coordinate - * @param Dcamera is the Jacobian w.r.t. [pose3 calibration] - * @param Dpoint is the Jacobian w.r.t. point3 - */ - Point2 project2(const Point3& pw, // - boost::optional Dcamera, boost::optional Dpoint) const { - - const Point3 pc = pose_.transform_to(pw); - const Point2 pn = project_to_camera(pc); - - if (!Dcamera && !Dpoint) { - return K_.uncalibrate(pn); - } else { - const double z = pc.z(), d = 1.0 / z; - - // uncalibration - Matrix2K Dcal; - Matrix2 Dpi_pn; - const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); - - if (Dcamera) { - Dcamera->resize(2, this->dim()); - calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>()); - Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib - } - if (Dpoint) { - Dpoint->resize(2, 3); - calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); - } - return pi; - } - } - /// backproject a 2-dimensional point to a 3-dimensional point at given depth inline Point3 backproject(const Point2& p, double depth) const { const Point2 pn = K_.calibrate(p); @@ -520,71 +363,64 @@ class PinholeCamera { /** * Calculate range to a landmark * @param point 3D location of landmark - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dpoint the optionally computed Jacobian with respect to the landmark * @return range (double) */ double range( const Point3& point, // - boost::optional Dpose = boost::none, - boost::optional Dpoint = boost::none) const { - double result = pose_.range(point, Dpose, Dpoint); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); - } + OptionalJacobian<1, dimension> Dcamera = boost::none, + OptionalJacobian<1, 3> Dpoint = boost::none) const { + Matrix16 Dpose_; + double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); + if (Dcamera) + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dpose2 the optionally computed Jacobian with respect to the other pose * @return range (double) */ double range( const Pose3& pose, // - boost::optional Dpose = boost::none, - boost::optional Dpose2 = boost::none) const { - double result = pose_.range(pose, Dpose, Dpose2); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); - } + OptionalJacobian<1, dimension> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose = boost::none) const { + Matrix16 Dpose_; + double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); + if (Dcamera) + *Dcamera << Dpose_, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ template double range( const PinholeCamera& camera, // - boost::optional Dpose = boost::none, - boost::optional Dother = boost::none) const { - double result = pose_.range(camera.pose_, Dpose, Dother); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); - H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); + OptionalJacobian<1, dimension> Dcamera = boost::none, +// OptionalJacobian<1, 6 + traits::dimension::value> Dother = + boost::optional Dother = + boost::none) const { + Matrix16 Dcamera_, Dother_; + double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0, + Dother ? &Dother_ : 0); + if (Dcamera) { + Dcamera->resize(1, 6 + DimK); + *Dcamera << Dcamera_, Eigen::Matrix::Zero(); } if (Dother) { - // Add columns of zeros to Jacobian for calibration - Matrix& H2r(*Dother); - H2r.conservativeResize(Eigen::NoChange, - camera.pose().dim() + camera.calibration().dim()); - H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) = - Matrix::Zero(1, camera.calibration().dim()); + Dother->resize(1, 6+CalibrationB::dimension); + Dother->setZero(); + Dother->block(0, 0, 1, 6) = Dother_; } return result; } @@ -592,15 +428,15 @@ class PinholeCamera { /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ double range( const CalibratedCamera& camera, // - boost::optional Dpose = boost::none, - boost::optional Dother = boost::none) const { - return pose_.range(camera.pose_, Dpose, Dother); + OptionalJacobian<1, dimension> Dcamera = boost::none, + OptionalJacobian<1, 6> Dother = boost::none) const { + return range(camera.pose(), Dcamera, Dother); } private: @@ -659,26 +495,8 @@ class PinholeCamera { }; -// 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 8b90aed63d..4b2111efc1 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; @@ -38,15 +34,9 @@ bool Point2::equals(const Point2& q, double tol) const { } /* ************************************************************************* */ -double Point2::norm() const { - return sqrt(x_ * x_ + y_ * y_); -} - -/* ************************************************************************* */ -double Point2::norm(boost::optional H) const { - double r = norm(); +double Point2::norm(OptionalJacobian<1,2> H) const { + double r = sqrt(x_ * x_ + y_ * y_); if (H) { - H->resize(1,2); if (fabs(r) > 1e-10) *H << x_ / r, y_ / r; else @@ -56,12 +46,11 @@ double Point2::norm(boost::optional H) const { } /* ************************************************************************* */ -static const Matrix I2 = eye(2); -double Point2::distance(const Point2& point, boost::optional H1, - boost::optional H2) const { +double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1, + OptionalJacobian<1,2> H2) const { Point2 d = point - *this; if (H1 || H2) { - Matrix H; + Matrix12 H; double r = d.norm(H); if (H1) *H1 = -H; if (H2) *H2 = H; diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index a4e0cc296e..6f4f93cf9d 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 @@ class GTSAM_EXPORT Point2 { /// @{ /// 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 @@ class GTSAM_EXPORT Point2 { /// @{ /// identity - inline static Point2 identity() { - return Point2(); - } + inline static Point2 identity() {return Point2();} - /// "Inverse" - negates each coordinate such that compose(p,inverse(p)) == identity() - inline Point2 inverse() const { return Point2(-x_, -y_); } - - /// syntactic sugar for inverse, i.e., -p == inverse(p) + /// inverse inline Point2 operator- () const {return Point2(-x_,-y_);} - /// "Compose", just adds the coordinates of two points. With optional derivatives - inline Point2 compose(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = eye(2); - if(H2) *H2 = eye(2); - return *this + q; - } - - /// syntactic sugar for adding two points, i.e., p+q == compose(p,q) + /// add inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);} - /// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q) - inline Point2 between(const Point2& q, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(2); - if(H2) *H2 = eye(2); - return q - (*this); - } - - /// syntactic sugar for subtracting points, i.e., q-p == between(p,q) + /// subtract inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);} - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return 2; } - - /// Dimensionality of tangent space = 2 DOF - inline size_t dim() const { return 2; } - - /// Updates a with tangent space delta - inline Point2 retract(const Vector& v) const { return *this + Point2(v); } - - /// Local coordinates of manifold neighborhood around current value - inline Vector localCoordinates(const Point2& t2) const { return Logmap(between(t2)); } - - /// @} - /// @name Lie Group - /// @{ - - /// Exponential map around identity - just create a Point2 from a vector - static inline Point2 Expmap(const Vector& v) { return Point2(v); } - - /// Log map around identity - just return the Point2 as a vector - static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); } - /// @} /// @name Vector Space /// @{ @@ -180,15 +125,12 @@ class GTSAM_EXPORT Point2 { /** creates a unit vector */ Point2 unit() const { return *this/norm(); } - /** norm of point */ - double norm() const; - /** norm of point, with derivative */ - double norm(boost::optional H) const; + double norm(OptionalJacobian<1,2> H = boost::none) const; /** distance between two points */ - double distance(const Point2& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none, + OptionalJacobian<1,2> H2 = boost::none) const; /** @deprecated The following function has been deprecated, use distance above */ inline double dist(const Point2& p2) const { @@ -218,10 +160,18 @@ class GTSAM_EXPORT Point2 { Vector2 vector() const { return Vector2(x_, y_); } /// @} - /// @name Deprecated (non-const, non-functional style. Do not use). + + /// @name Deprecated /// @{ inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;} inline void operator *= (double s) {x_*=s;y_*=s;} + Point2 inverse() const { return -(*this);} + Point2 compose(const Point2& q) const { return (*this)+q;} + Point2 between(const Point2& q) const { return q-(*this);} + Vector2 localCoordinates(const Point2& q) const { return between(q).vector();} + Point2 retract(const Vector2& v) const { return compose(Point2(v));} + static Vector2 Logmap(const Point2& p) { return p.vector();} + static Point2 Expmap(const Vector2& v) { return Point2(v);} /// @} /// Streaming @@ -248,22 +198,8 @@ class GTSAM_EXPORT Point2 { /// 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{ -}; - -template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ -}; - -template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ -}; - -} +struct traits : public internal::VectorSpace {}; -} +} // \ namespace gtsam diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index ce56c78c12..a87aeb650c 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 @@ -63,22 +59,18 @@ Point3 Point3::operator/(double s) const { } /* ************************************************************************* */ -Point3 Point3::add(const Point3 &q, boost::optional H1, - boost::optional H2) const { - if (H1) - *H1 = eye(3, 3); - if (H2) - *H2 = eye(3, 3); +Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { + if (H1) *H1 = I_3x3; + if (H2) *H2 = I_3x3; return *this + q; } /* ************************************************************************* */ -Point3 Point3::sub(const Point3 &q, boost::optional H1, - boost::optional H2) const { - if (H1) - *H1 = eye(3, 3); - if (H2) - *H2 = -eye(3, 3); +Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { + if (H1) *H1 = I_3x3; + if (H2) *H2 = I_3x3; return *this - q; } @@ -94,26 +86,8 @@ double Point3::dot(const Point3 &q) const { } /* ************************************************************************* */ -double Point3::norm() const { - return sqrt(x_ * x_ + y_ * y_ + z_ * z_); -} - -/* ************************************************************************* */ -double Point3::norm(boost::optional H) const { - double r = norm(); - if (H) { - H->resize(1,3); - if (fabs(r) > 1e-10) - *H << x_ / r, y_ / r, z_ / r; - else - *H << 1, 1, 1; // really infinity, why 1 ? - } - return r; -} - -/* ************************************************************************* */ -double Point3::norm(boost::optional&> H) const { - double r = norm(); +double Point3::norm(OptionalJacobian<1,3> H) const { + double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_); if (H) { if (fabs(r) > 1e-10) *H << x_ / r, y_ / r, z_ / r; @@ -124,13 +98,12 @@ double Point3::norm(boost::optional&> H) const { } /* ************************************************************************* */ -Point3 Point3::normalize(boost::optional H) const { +Point3 Point3::normalize(OptionalJacobian<3,3> H) const { Point3 normalized = *this / norm(); if (H) { // 3*3 Derivative double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_; double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_; - H->resize(3, 3); *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2; *H /= pow(x2 + y2 + z2, 1.5); } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index b7f7f8ffa6..c9dee90035 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, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if (H1) *H1 = eye(3); - if (H2) *H2 = eye(3); - return *this + p2; - } - - ///syntactic sugar for adding two points, i.e., p+q == compose(p,q) + /// add Point3 operator + (const Point3& q) const; - /** Between using the default implementation */ - inline Point3 between(const Point3& p2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - if(H1) *H1 = -eye(3); - if(H2) *H2 = eye(3); - return p2 - *this; - } - - /// syntactic sugar for subtracting points, i.e., q-p == between(p,q) + /// subtract Point3 operator - (const Point3& q) const; - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return 3; } - - /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return 3; } - - /// Updates a with tangent space delta - inline Point3 retract(const Vector& v) const { return Point3(*this + v); } - - /// Returns inverse retraction - inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } - - /// @} - /// @name Lie Group - /// @{ - - /** Exponential map at identity - just create a Point3 from x,y,z */ - static inline Point3 Expmap(const Vector& v) { return Point3(v); } - - /** Log map at identity - return the x,y,z of this point */ - static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); } - - /// Left-trivialized derivative of the exponential map - static Matrix dexpL(const Vector& v) { - return eye(3); - } - - /// Left-trivialized derivative inverse of the exponential map - static Matrix dexpInvL(const Vector& v) { - return eye(3); - } - /// @} /// @name Vector Space /// @{ @@ -163,14 +99,16 @@ namespace gtsam { /** distance between two points */ inline double distance(const Point3& p2, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const { double d = (p2 - *this).norm(); if (H1) { - *H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z()).finished()*(1./d); + *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z(); + *H1 = *H1 *(1./d); } if (H2) { - *H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z()).finished()*(1./d); + *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z(); + *H2 << *H2 *(1./d); } return d; } @@ -180,17 +118,11 @@ namespace gtsam { return (p2 - *this).norm(); } - /** Distance of the point from the origin */ - double norm() const; - /** Distance of the point from the origin, with Jacobian */ - double norm(boost::optional H) const; - - /** Distance of the point from the origin, with Jacobian */ - double norm(boost::optional&> H) const; + double norm(OptionalJacobian<1,3> H = boost::none) const; /** normalize, with optional Jacobian */ - Point3 normalize(boost::optional H = boost::none) const; + Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const; /** cross product @return this x q */ Point3 cross(const Point3 &q) const; @@ -219,17 +151,28 @@ namespace gtsam { /** add two points, add(this,q) is same as this + q */ Point3 add (const Point3 &q, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const; /** subtract two points, sub(this,q) is same as this - q */ Point3 sub (const Point3 &q, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const; /// @} /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point3& p); + /// @name Deprecated + /// @{ + Point3 inverse() const { return -(*this);} + Point3 compose(const Point3& q) const { return (*this)+q;} + Point3 between(const Point3& q) const { return q-(*this);} + Vector3 localCoordinates(const Point3& q) const { return between(q).vector();} + Point3 retract(const Vector3& v) const { return compose(Point3(v));} + static Vector3 Logmap(const Point3& p) { return p.vector();} + static Point3 Expmap(const Vector3& v) { return Point3(v);} + /// @} + private: /// @name Advanced Interface @@ -252,20 +195,6 @@ namespace gtsam { /// Syntactic sugar for multiplying coordinates by a scalar s*p inline Point3 operator*(double s, const Point3& p) { return p*s;} - // Define GTSAM traits - namespace traits { - template<> - struct GTSAM_EXPORT is_group : 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 7a693ed3df..0b0172857f 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,21 +29,23 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Pose2); - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose2); -static const Matrix I3 = eye(3), Z12 = zeros(1,2); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); /* ************************************************************************* */ -Matrix Pose2::matrix() const { - Matrix R = r_.matrix(); - R = stack(2, &R, &Z12); - Matrix T = (Matrix(3, 1) << t_.x(), t_.y(), 1.0).finished(); - return collect(2, &R, &T); +Matrix3 Pose2::matrix() const { + Matrix2 R = r_.matrix(); + Matrix32 R0; + R0.block<2,2>(0,0) = R; + R0.block<1,2>(2,0).setZero(); + Matrix31 T; + T << t_.x(), t_.y(), 1.0; + Matrix3 RT_; + RT_.block<3,2>(0,0) = R0; + RT_.block<3,1>(0,2) = T; + return RT_; } /* ************************************************************************* */ @@ -55,7 +59,8 @@ bool Pose2::equals(const Pose2& q, double tol) const { } /* ************************************************************************* */ -Pose2 Pose2::Expmap(const Vector& xi) { +Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) { + if (H) *H = Pose2::ExpmapDerivative(xi); assert(xi.size() == 3); Point2 v(xi(0),xi(1)); double w = xi(2); @@ -70,260 +75,222 @@ Pose2 Pose2::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector Pose2::Logmap(const Pose2& p) { +Vector3 Pose2::Logmap(const Pose2& p, OptionalJacobian<3, 3> H) { + if (H) *H = Pose2::LogmapDerivative(p); const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); if (std::abs(w) < 1e-10) - return (Vector(3) << t.x(), t.y(), w).finished(); + return Vector3(t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); double det = c_1*c_1 + s*s; Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 v = (w/det) * p; - return (Vector(3) << v.x(), v.y(), w).finished(); + return Vector3(v.x(), v.y(), w); } } /* ************************************************************************* */ -Pose2 Pose2::retract(const Vector& v) const { +Pose2 Pose2::ChartAtOrigin::Retract(const Vector3& v, ChartJacobian H) { #ifdef SLOW_BUT_CORRECT_EXPMAP - return compose(Expmap(v)); + return Expmap(v, H); #else - assert(v.size() == 3); - return compose(Pose2(v[0], v[1], v[2])); + if (H) { + *H = I_3x3; + H->topLeftCorner<2,2>() = Rot2(-v[2]).matrix(); + } + return Pose2(v[0], v[1], v[2]); #endif } - /* ************************************************************************* */ -Vector Pose2::localCoordinates(const Pose2& p2) const { +Vector3 Pose2::ChartAtOrigin::Local(const Pose2& r, ChartJacobian H) { #ifdef SLOW_BUT_CORRECT_EXPMAP - return Logmap(between(p2)); + return Logmap(r, H); #else - Pose2 r = between(p2); - return (Vector(3) << r.x(), r.y(), r.theta()).finished(); + if (H) { + *H = I_3x3; + H->topLeftCorner<2,2>() = r.rotation().matrix(); + } + return Vector3(r.x(), r.y(), r.theta()); #endif } /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi) -Matrix Pose2::AdjointMap() const { +Matrix3 Pose2::AdjointMap() const { double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y(); - return (Matrix(3, 3) << + Matrix3 rvalue; + rvalue << c, -s, y, s, c, -x, - 0.0, 0.0, 1.0 - ).finished(); + 0.0, 0.0, 1.0; + return rvalue; } /* ************************************************************************* */ -Pose2 Pose2::inverse(boost::optional H1) const { - if (H1) *H1 = -AdjointMap(); - return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); +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; } /* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Point2 Pose2::transform_to(const Point2& point) const { - Point2 d = point - t_; - return r_.unrotate(d); +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; } /* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = point - t_; - Point2 q = r_.unrotate(d); - if (!H1 && !H2) return q; - if (H1) *H1 << - -1.0, 0.0, q.y(), - 0.0, -1.0, -q.x(); - if (H2) *H2 = r_.transpose(); - return q; +Matrix3 Pose2::LogmapDerivative(const Pose2& p) { + Vector3 v = Logmap(p); + double alpha = v[2]; + Matrix3 J; + if (fabs(alpha) > 1e-5) { + double alphaInv = 1/alpha; + double halfCotHalfAlpha = 0.5*sin(alpha)/(1-cos(alpha)); + double v1 = v[0], v2 = v[1]; + + J << alpha*halfCotHalfAlpha, -0.5*alpha, v1*alphaInv - v1*halfCotHalfAlpha + 0.5*v2, + 0.5*alpha, alpha*halfCotHalfAlpha, v2*alphaInv - 0.5*v1 - v2*halfCotHalfAlpha, + 0, 0, 1; + } + else { + J << 1,0, 0.5*v[1], + 0,1, -0.5*v[0], + 0,0, 1; + } + return J; +} + +/* ************************************************************************* */ +Pose2 Pose2::inverse() const { + return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { Point2 d = point - t_; Point2 q = r_.unrotate(d); if (!H1 && !H2) return q; - if (H1) *H1 = (Matrix(2, 3) << + if (H1) *H1 << -1.0, 0.0, q.y(), - 0.0, -1.0, -q.x()).finished(); - if (H2) *H2 = r_.transpose(); + 0.0, -1.0, -q.x(); + if (H2) *H2 << r_.transpose(); return q; } -/* ************************************************************************* */ -// see doc/math.lyx, SE(2) section -Pose2 Pose2::compose(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // TODO: inline and reuse? - if(H1) *H1 = p2.inverse().AdjointMap(); - if(H2) *H2 = I3; - return (*this)*p2; -} - /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_from(const Point2& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = r_ * p; if (H1 || H2) { - const Matrix R = r_.matrix(); - const Matrix Drotate1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); - if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q] - if (H2) *H2 = R; // R + const Matrix2 R = r_.matrix(); + Matrix21 Drotate1; + Drotate1 << -q.y(), q.x(); + if (H1) *H1 << R, Drotate1; + if (H2) *H2 = R; // R } return q + t_; } -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - return Pose2(R,t); -} - -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above - if (H1) { - double dt1 = -s2 * x + c2 * y; - double dt2 = -c2 * x - s2 * y; - *H1 << - -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0,-1.0; - } - if (H2) *H2 = I3; - - return Pose2(R,t); -} - -/* ************************************************************************* */ -Pose2 Pose2::between(const Pose2& p2, boost::optional H1, - boost::optional H2) const { - // get cosines and sines from rotation matrices - const Rot2& R1 = r_, R2 = p2.r(); - double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); - - // Assert that R1 and R2 are normalized - assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); - - // Calculate delta rotation = between(R1,R2) - double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; - Rot2 R(Rot2::atan2(s,c)); // normalizes - - // Calculate delta translation = unrotate(R1, dt); - Point2 dt = p2.t() - t_; - double x = dt.x(), y = dt.y(); - // t = R1' * (t2-t1) - Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); - - // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above - if (H1) { - double dt1 = -s2 * x + c2 * y; - double dt2 = -c2 * x - s2 * y; - *H1 = (Matrix(3, 3) << - -c, -s, dt1, - s, -c, dt2, - 0.0, 0.0,-1.0).finished(); - } - if (H2) *H2 = I3; - - return Pose2(R,t); -} - /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, - boost::optional H1, boost::optional H2) const { - Point2 d = transform_to(point, H1, H2); + OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const { + // make temporary matrices + Matrix23 D1; Matrix2 D2; + Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version if (!H1 && !H2) return Rot2::relativeBearing(d); - Matrix D_result_d; + Matrix12 D_result_d; Rot2 result = Rot2::relativeBearing(d, D_result_d); - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); + if (H1) *H1 = D_result_d * (D1); + if (H2) *H2 = D_result_d * (D2); return result; } /* ************************************************************************* */ -Rot2 Pose2::bearing(const Pose2& point, - boost::optional H1, boost::optional H2) const { - Rot2 result = bearing(point.t(), H1, H2); +Rot2 Pose2::bearing(const Pose2& pose, + OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const { + Matrix12 D2; + Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0); if (H2) { - Matrix H2_ = *H2 * point.r().matrix(); - *H2 = zeros(1, 3); - insertSub(*H2, H2_, 0, 0); + Matrix12 H2_ = D2 * pose.r().matrix(); + *H2 << H2_, Z_1x1; } return result; } - /* ************************************************************************* */ double Pose2::range(const Point2& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const { Point2 d = point - t_; if (!H1 && !H2) return d.norm(); - Matrix H; + Matrix12 H; double r = d.norm(H); - if (H1) *H1 = H * (Matrix(2, 3) << - -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0).finished(); + if (H1) { + Matrix23 H1_; + H1_ << -r_.c(), r_.s(), 0.0, + -r_.s(), -r_.c(), 0.0; + *H1 = H * H1_; + } if (H2) *H2 = H; return r; } /* ************************************************************************* */ -double Pose2::range(const Pose2& pose2, - boost::optional H1, - boost::optional H2) const { - Point2 d = pose2.t() - t_; +double Pose2::range(const Pose2& pose, + OptionalJacobian<1,3> H1, + OptionalJacobian<1,3> H2) const { + Point2 d = pose.t() - t_; if (!H1 && !H2) return d.norm(); - Matrix H; + Matrix12 H; double r = d.norm(H); - if (H1) *H1 = H * (Matrix(2, 3) << + if (H1) { + Matrix23 H1_; + H1_ << -r_.c(), r_.s(), 0.0, - -r_.s(), -r_.c(), 0.0).finished(); - if (H2) *H2 = H * (Matrix(2, 3) << - pose2.r_.c(), -pose2.r_.s(), 0.0, - pose2.r_.s(), pose2.r_.c(), 0.0).finished(); + -r_.s(), -r_.c(), 0.0; + *H1 = H * H1_; + } + if (H2) { + Matrix23 H2_; + H2_ << + pose.r_.c(), -pose.r_.s(), 0.0, + pose.r_.s(), pose.r_.c(), 0.0; + *H2 = H * H2_; + } return r; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index d43be6afb4..be860107e3 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 @@ class GTSAM_EXPORT Pose2 { typedef Point2 Translation; private: + Rot2 r_; Point2 t_; @@ -106,72 +105,39 @@ class GTSAM_EXPORT Pose2 { /// identity for group operation inline static Pose2 identity() { return Pose2(); } - /// inverse transformation with derivatives - Pose2 inverse(boost::optional H1=boost::none) const; - - /// compose this transformation onto another (first *this and then p2) - Pose2 compose(const Pose2& p2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + /// inverse + Pose2 inverse() const; /// compose syntactic sugar inline Pose2 operator*(const Pose2& p2) const { return Pose2(r_*p2.r(), t_ + r_*p2.t()); } - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2) const; - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, boost::optional H1, - boost::optional H2) const; - - /** - * Return relative pose between p1 and p2, in p1 coordinate frame - */ - Pose2 between(const Pose2& p2, boost::optional H1, - boost::optional H2) const; - - /// @} - /// @name Manifold - /// @{ - - /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes - inline static size_t Dim() { return 3; } - - /// Dimensionality of tangent space = 3 DOF - inline size_t dim() const { return 3; } - - /// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose - Pose2 retract(const Vector& v) const; - - /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose - Vector localCoordinates(const Pose2& p2) const; - /// @} /// @name Lie Group /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - static Pose2 Expmap(const Vector& xi); + static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation - static Vector Logmap(const Pose2& p); + static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); /** * Calculate Adjoint map * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ - Matrix AdjointMap() const; + Matrix3 AdjointMap() const; inline Vector Adjoint(const Vector& xi) const { assert(xi.size() == 3); return AdjointMap()*xi; } + /** + * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 + */ + static Matrix3 adjointMap(const Vector& v); + /** * wedge for SE(2): * @param xi 3-dim twist (v,omega) where @@ -179,34 +145,41 @@ class GTSAM_EXPORT Pose2 { * v (vx,vy) = 2D velocity * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ - static inline Matrix wedge(double vx, double vy, double w) { - return (Matrix(3,3) << - 0.,-w, vx, - w, 0., vy, - 0., 0., 0.).finished(); + static inline Matrix3 wedge(double vx, double vy, double w) { + Matrix3 m; + m << 0.,-w, vx, + w, 0., vy, + 0., 0., 0.; + return m; } + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& v); + + /// Derivative of Logmap + static Matrix3 LogmapDerivative(const Pose2& v); + + // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP + struct ChartAtOrigin { + static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none); + static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none); + }; + + using LieGroup::inverse; // version with derivative + /// @} /// @name Group Action on Point2 /// @{ - /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point) const; - /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, - boost::optional H1, - boost::optional H2) const; - - /** Return point coordinates in pose coordinate frame */ - Point2 transform_to(const Point2& point, - boost::optional H1, - boost::optional H2) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<2, 3> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** syntactic sugar for transform_from */ inline Point2 operator*(const Point2& point) const { return transform_from(point);} @@ -237,7 +210,7 @@ class GTSAM_EXPORT Pose2 { inline const Rot2& rotation() const { return r_; } //// return transformation matrix - Matrix matrix() const; + Matrix3 matrix() const; /** * Calculate bearing to a landmark @@ -245,17 +218,15 @@ class GTSAM_EXPORT Pose2 { * @return 2D rotation \f$ \in SO(2) \f$ */ Rot2 bearing(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const; /** * Calculate bearing to another pose * @param point SO(2) location of other pose * @return 2D rotation \f$ \in SO(2) \f$ */ - Rot2 bearing(const Pose2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + Rot2 bearing(const Pose2& pose, + OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const; /** * Calculate range to a landmark @@ -263,8 +234,8 @@ class GTSAM_EXPORT Pose2 { * @return range (double) */ double range(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, + OptionalJacobian<1, 2> H2=boost::none) const; /** * Calculate range to another pose @@ -272,8 +243,8 @@ class GTSAM_EXPORT Pose2 { * @return range (double) */ double range(const Pose2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1, 3> H1=boost::none, + OptionalJacobian<1, 3> H2=boost::none) const; /// @} /// @name Advanced Interface @@ -319,18 +290,8 @@ inline Matrix wedge(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 b134553d98..be8e1bfeda 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -17,7 +17,8 @@ #include #include #include -#include +#include + #include #include #include @@ -26,21 +27,21 @@ using namespace std; namespace gtsam { -/** Explicit instantiation of base class to export members */ -INSTANTIATE_LIE(Pose3); - /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); -static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3; -static const Matrix6 I6 = eye(6); - /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : R_(Rot3::rodriguez(0, 0, pose2.theta())), t_( Point3(pose2.x(), pose2.y(), 0)) { } +/* ************************************************************************* */ +Pose3 Pose3::inverse() const { + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt * (-t_)); +} + /* ************************************************************************* */ // Calculate Adjoint map // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) @@ -50,69 +51,52 @@ Matrix6 Pose3::AdjointMap() const { const Vector3 t = t_.vector(); Matrix3 A = skewSymmetric(t) * R; Matrix6 adj; - adj << R, Z3, A, R; + adj << R, Z_3x3, A, R; return adj; } /* ************************************************************************* */ -Matrix6 Pose3::adjointMap(const Vector& xi) { +Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix6 adj; - adj << w_hat, Z3, v_hat, w_hat; + adj << w_hat, Z_3x3, v_hat, w_hat; return adj; } /* ************************************************************************* */ -Vector Pose3::adjoint(const Vector& xi, const Vector& y, - boost::optional H) { +Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + H->setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; - Matrix Gi = adjointMap(dxi); - (*H).col(i) = Gi * y; + Matrix6 Gi = adjointMap(dxi); + H->col(i) = Gi * y; } } return adjointMap(xi) * y; } /* ************************************************************************* */ -Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, - boost::optional H) { +Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + H->setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; - Matrix GTi = adjointMap(dxi).transpose(); - (*H).col(i) = GTi * y; + Matrix6 GTi = adjointMap(dxi).transpose(); + H->col(i) = GTi * y; } } - Matrix adjT = adjointMap(xi).transpose(); return adjointMap(xi).transpose() * y; } -/* ************************************************************************* */ -Matrix6 Pose3::dExpInv_exp(const Vector& xi) { - // Bernoulli numbers, from Wikipedia - static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, - 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); - static const int N = 5; // order of approximation - Matrix res = I6; - Matrix6 ad_i = I6; - Matrix6 ad_xi = adjointMap(xi); - double fac = 1.0; - for (int i = 1; i < N; ++i) { - ad_i = ad_xi * ad_i; - fac = fac * i; - res = res + B(i) / fac * ad_i; - } - return res; -} - /* ************************************************************************* */ void Pose3::print(const string& s) const { cout << s; @@ -127,7 +111,8 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector& xi) { +Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { + if (H) *H = ExpmapDerivative(xi); // get angular velocity omega and translational velocity v from twist xi Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); @@ -147,7 +132,8 @@ Pose3 Pose3::Expmap(const Vector& xi) { } /* ************************************************************************* */ -Vector6 Pose3::Logmap(const Pose3& p) { +Vector6 Pose3::Logmap(const Pose3& p, OptionalJacobian<6, 6> H) { + if (H) *H = LogmapDerivative(p); Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); double t = w.norm(); if (t < 1e-10) { @@ -168,64 +154,111 @@ Vector6 Pose3::Logmap(const Pose3& p) { } /* ************************************************************************* */ -Pose3 Pose3::retractFirstOrder(const Vector& xi) const { - Vector3 omega(sub(xi, 0, 3)); - Point3 v(sub(xi, 3, 6)); - Rot3 R = R_.retract(omega); // R is done exactly - Point3 t = t_ + R_ * v; // First order t approximation - return Pose3(R, t); +Pose3 Pose3::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, H); +#else + Matrix3 DR; + Rot3 R = Rot3::Retract(xi.head<3>(), H ? &DR : 0); + if (H) { + *H = I_6x6; + H->topLeftCorner<3,3>() = DR; + } + return Pose3(R, Point3(xi.tail<3>())); +#endif } /* ************************************************************************* */ -// Different versions of retract -Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group exponential map, traces out geodesic - return compose(Expmap(xi)); - } else if (mode == Pose3::FIRST_ORDER) { - // First order - return retractFirstOrder(xi); - } else { - // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently - // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation - assert(false); - exit(1); +Vector6 Pose3::ChartAtOrigin::Local(const Pose3& T, ChartJacobian H) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(T, H); +#else + Matrix3 DR; + Vector3 omega = Rot3::LocalCoordinates(T.rotation(), H ? &DR : 0); + if (H) { + *H = I_6x6; + H->topLeftCorner<3,3>() = DR; } + Vector6 xi; + xi << omega, T.translation().vector(); + return xi; +#endif } /* ************************************************************************* */ -// different versions of localCoordinates -Vector6 Pose3::localCoordinates(const Pose3& T, - Pose3::CoordinatesMode mode) const { - if (mode == Pose3::EXPMAP) { - // Lie group logarithm map, exact inverse of exponential map - return Logmap(between(T)); - } else if (mode == Pose3::FIRST_ORDER) { - // R is always done exactly in all three retract versions below - Vector3 omega = R_.localCoordinates(T.rotation()); - - // Incorrect version - // Independently computes the logmap of the translation and rotation - // Vector v = t_.localCoordinates(T.translation()); - - // Correct first order t inverse - Point3 d = R_.unrotate(T.translation() - t_); - - // TODO: correct second order t inverse - Vector6 local; - local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z(); - return local; - } else { - assert(false); - exit(1); +/** + * 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); + + Matrix3 Q; + +#ifdef NUMERICAL_EXPMAP_DERIV + Matrix3 Qj = Z_3x3; + double invFac = 1.0; + Q = Z_3x3; + Matrix3 Wj = I_3x3; + for (size_t j=1; j<10; ++j) { + Qj = Qj*W + Wj*V; + invFac = -invFac/(j+1); + Q = Q + invFac*Qj; + Wj = Wj*W; + } +#else + // The closed-form formula in Barfoot14tro eq. (102) + double phi = w.norm(); + if (fabs(phi)>1e-5) { + double sinPhi = sin(phi), cosPhi = cos(phi); + double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Q = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); } + else { + Q = -0.5*V + 1./6.*(W*V + V*W - W*V*W) + + 1./24.*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + } +#endif + + return Q; +} + +/* ************************************************************************* */ +Matrix6 Pose3::ExpmapDerivative(const Vector6& xi) { + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::ExpmapDerivative(w); + Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q, Jw).finished(); + return J; +} + +/* ************************************************************************* */ +Matrix6 Pose3::LogmapDerivative(const Pose3& pose) { + Vector6 xi = Logmap(pose); + Vector3 w(sub(xi, 0, 3)); + Matrix3 Jw = Rot3::LogmapDerivative(w); + Matrix3 Q = computeQforExpmapDerivative(xi); + Matrix3 Q2 = -Jw*Q*Jw; + Matrix6 J = (Matrix(6,6) << Jw, Z_3x3, Q2, Jw).finished(); + return J; } /* ************************************************************************* */ Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); const Vector3 T = t_.vector(); - Eigen::Matrix A14; + Matrix14 A14; A14 << 0.0, 0.0, 0.0, 1.0; Matrix4 mat; mat << R, T, A14; @@ -240,12 +273,11 @@ Pose3 Pose3::transform_to(const Pose3& pose) const { } /* ************************************************************************* */ -Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { +Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { if (Dpose) { const Matrix3 R = R_.matrix(); Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - Dpose->resize(3, 6); (*Dpose) << DR, R; } if (Dpoint) @@ -254,13 +286,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p) const { - return R_.unrotate(p - t_); -} - -/* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { +Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); @@ -278,77 +305,32 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, } /* ************************************************************************* */ -Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, - boost::optional Dpoint) const { - const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p - t_).vector()); - if (Dpose) { - const double wx = q.x(), wy = q.y(), wz = q.z(); - Dpose->resize(3, 6); - (*Dpose) << - 0.0, -wz, +wy,-1.0, 0.0, 0.0, - +wz, 0.0, -wx, 0.0,-1.0, 0.0, - -wy, +wx, 0.0, 0.0, 0.0,-1.0; - } - if (Dpoint) - *Dpoint = Rt; - return q; -} - -/* ************************************************************************* */ -Pose3 Pose3::compose(const Pose3& p2, boost::optional H1, - boost::optional H2) const { - if (H1) - *H1 = p2.inverse().AdjointMap(); - if (H2) - *H2 = I6; - return (*this) * p2; -} - -/* ************************************************************************* */ -Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) - *H1 = -AdjointMap(); - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt * (-t_)); -} - -/* ************************************************************************* */ -// between = compose(p2,inverse(p1)); -Pose3 Pose3::between(const Pose3& p2, boost::optional H1, - boost::optional H2) const { - Pose3 result = inverse() * p2; - if (H1) - *H1 = -result.inverse().AdjointMap(); - if (H2) - *H2 = I6; - return result; -} - -/* ************************************************************************* */ -double Pose3::range(const Point3& point, boost::optional H1, - boost::optional H2) const { +double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1, + OptionalJacobian<1, 3> H2) const { if (!H1 && !H2) return transform_to(point).norm(); - Point3 d = transform_to(point, H1, H2); - double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt( - d2); - Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished(); - if (H1) - *H1 = D_result_d * (*H1); - if (H2) - *H2 = D_result_d * (*H2); - return n; + else { + Matrix36 D1; + Matrix3 D2; + Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); + const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, + n = sqrt(d2); + Matrix13 D_result_d; + D_result_d << x / n, y / n, z / n; + if (H1) *H1 = D_result_d * D1; + if (H2) *H2 = D_result_d * D2; + return n; + } } /* ************************************************************************* */ -double Pose3::range(const Pose3& point, boost::optional H1, - boost::optional H2) const { - double r = range(point.translation(), H1, H2); +double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, + OptionalJacobian<1,6> H2) const { + Matrix13 D2; + double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { - Matrix H2_ = *H2 * point.rotation().matrix(); - *H2 = zeros(1, 6); - insertSub(*H2, H2_, 0, 3); + Matrix13 H2_ = D2 * pose.rotation().matrix(); + *H2 << Matrix13::Zero(), H2_; } return r; } @@ -370,7 +352,7 @@ boost::optional align(const vector& pairs) { cq *= f; // Add to form H matrix - Matrix H = zeros(3, 3); + Matrix3 H = Eigen::Matrix3d::Zero(); BOOST_FOREACH(const Point3Pair& pair, pairs){ Vector dp = pair.first.vector() - cp; Vector dq = pair.second.vector() - cq; @@ -378,13 +360,13 @@ boost::optional align(const vector& pairs) { } // Compute SVD - Matrix U, V; + Matrix U,V; Vector S; svd(H, U, S, V); // Recover transform with correction from Eggert97machinevisionandapplications - Matrix UVtranspose = U * V.transpose(); - Matrix detWeighting = eye(3, 3); + Matrix3 UVtranspose = U * V.transpose(); + Matrix3 detWeighting = I_3x3; detWeighting(2, 2) = UVtranspose.determinant(); Rot3 R(Matrix(V * detWeighting * U.transpose())); Point3 t = Point3(cq) - R * Point3(cp); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2090558a6a..d30bd41677 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 @@ class GTSAM_EXPORT Pose3{ private: - Rot3 R_; ///< Rotation gRp, between global and pose frame + Rot3 R_; ///< Rotation gRp, between global and pose frame Point3 t_; ///< Translation gTp, from global origin to pose frame origin public: @@ -70,6 +64,11 @@ class GTSAM_EXPORT Pose3{ R_(R), t_(t) { } + /** Construct from R,t, where t \in vector3 */ + Pose3(const Rot3& R, const Vector3& t) : + R_(R), t_(t) { + } + /** Construct from Pose2 */ explicit Pose3(const Pose2& pose2); @@ -99,247 +98,218 @@ class GTSAM_EXPORT Pose3{ } /// inverse transformation with derivatives - Pose3 inverse(boost::optional H1 = boost::none) const; - - ///compose this transformation onto another (first *this and then p2) - Pose3 compose(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional 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_); } + /// @} + /// @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, 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) + */ + 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); + /** - * Return relative pose between p1 and p2, in p1 coordinate frame - * as well as optionally the derivatives + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ - Pose3 between(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + 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); + }; + + using LieGroup::inverse; // version with derivative + + /** + * 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 Manifold + /// @name Group Action on Point3 /// @{ - /** Enum to indicate which method should be used in Pose3::retract() and - * Pose3::localCoordinates() + /** + * @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 */ - enum CoordinatesMode { - EXPMAP, ///< The correct exponential map, computationally expensive. - FIRST_ORDER ///< A fast first-order approximation to the exponential map. - }; + 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 + /// @{ - /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes - static size_t Dim() { - return 6; + /// get rotation + const Rot3& rotation() const { + return R_; } - /// Dimensionality of the tangent space = 6 DOF - size_t dim() const { - return 6; + /// get translation + const Point3& translation() const { + return t_; } - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map - Pose3 retractFirstOrder(const Vector& d) const; - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose - Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = - POSE3_DEFAULT_COORDINATES_MODE) const; - - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const; - - /// @} - /// @name Lie Group - /// @{ - - /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ - static Pose3 Expmap(const Vector& xi); - - /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation - static Vector6 Logmap(const Pose3& p); - - /** - * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame - * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) - */ - Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect - - /** - * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame - * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ - */ - Vector Adjoint(const Vector& xi_b) const {return AdjointMap()*xi_b; } /// FIXME Not tested - marked as incorrect - - /** - * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 - * [ad(w,v)] = [w^, zero3; v^, w^] - * Note that this is the matrix representation of the adjoint operator for se3 Lie algebra, - * aka the Lie bracket, and also the derivative of Adjoint map for the Lie group SE3. - * - * Let \f$ \hat{\xi}_i \f$ be the se3 Lie algebra, and \f$ \hat{\xi}_i^\vee = \xi_i = [\omega_i,v_i] \in \mathbb{R}^6\f$ be its - * vector representation. - * We have the following relationship: - * \f$ [\hat{\xi}_1,\hat{\xi}_2]^\vee = ad_{\xi_1}(\xi_2) = [ad_{(\omega_1,v_1)}]*\xi_2 \f$ - * - * We use this to compute the discrete version of the inverse right-trivialized tangent map, - * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. - * - */ - static Matrix6 adjointMap(const Vector& xi); - - /** - * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives - */ - static Vector adjoint(const Vector& xi, const Vector& y, boost::optional H = boost::none); - - /** - * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. - */ - static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional H = boost::none); - - /** - * Compute the inverse right-trivialized tangent (derivative) map of the exponential map, - * as detailed in [Kobilarov09siggraph] eq. (15) - * The full formula is documented in [Celledoni99cmame] - * Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and - * time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421-438, 2003. - * and in [Hairer06book] in formula (4.5), pg. 84, Lemma 4.2 - * Ernst Hairer, et al., Geometric Numerical Integration, - * Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006. - */ - static Matrix6 dExpInv_exp(const Vector& xi); - - /** - * wedge for Pose3: - * @param xi 6-dim twist (omega,v) where - * omega = (wx,wy,wz) 3D angular velocity - * v (vx,vy,vz) = 3D velocity - * @return xihat, 4*4 element of Lie algebra that can be exponentiated - */ - static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz) { - return (Matrix(4,4) << - 0.,-wz, wy, vx, - wz, 0.,-wx, vy, - -wy, wx, 0., vz, - 0., 0., 0., 0.).finished(); - } - - /// @} - /// @name Group Action on Point3 - /// @{ - - /** - * @brief takes point in Pose coordinates and transforms it to world coordinates - * @param p point in Pose coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point - * @return point in world coordinates - */ - Point3 transform_from(const Point3& p, - boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; - - /** syntactic sugar for transform_from */ - inline Point3 operator*(const Point3& p) const { return transform_from(p); } - - /** - * @brief takes point in world coordinates and transforms it to Pose coordinates - * @param p point in world coordinates - * @param Dpose optional 3*6 Jacobian wrpt this pose - * @param Dpoint optional 3*3 Jacobian wrpt point - * @return point in Pose coordinates - */ - Point3 transform_to(const Point3& p) const; - - Point3 transform_to(const Point3& p, - boost::optional Dpose, boost::optional Dpoint) const; - - Point3 transform_to(const Point3& p, - boost::optional Dpose, boost::optional Dpoint) const; - - /// @} - /// @name Standard Interface - /// @{ - - /// get rotation - const Rot3& rotation() const { return R_; } - - /// get translation - const Point3& translation() const { return t_; } - - /// get x - double x() const { return t_.x(); } - - /// get y - double y() const { return t_.y(); } - - /// get z - double z() const { return t_.z(); } - - /** convert to 4*4 matrix */ - Matrix4 matrix() const; - - /** receives a pose in world coordinates and transforms it to local coordinates */ - Pose3 transform_to(const Pose3& pose) const; - - /** - * Calculate range to a landmark - * @param point 3D location of landmark - * @return range (double) - */ - double range(const Point3& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - /** - * Calculate range to another pose - * @param pose Other SO(3) pose - * @return range (double) - */ - double range(const Pose3& pose, - boost::optional H1=boost::none, - boost::optional 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 + /// 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; /** - * 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 + * 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)); @@ -352,21 +322,7 @@ 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{ -}; - 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/Quaternion.h b/gtsam/geometry/Quaternion.h new file mode 100644 index 0000000000..534b4ab42e --- /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 0133c94401..41d56b6e3d 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) { @@ -64,21 +60,43 @@ Rot2& Rot2::normalize() { } /* ************************************************************************* */ -Matrix Rot2::matrix() const { - return (Matrix(2, 2) << c_, -s_, s_, c_).finished(); +Rot2 Rot2::Expmap(const Vector1& v, OptionalJacobian<1, 1> H) { + if (H) + *H = I_1x1; + if (zero(v)) + return (Rot2()); + else + return Rot2::fromAngle(v(0)); +} + +/* ************************************************************************* */ +Vector1 Rot2::Logmap(const Rot2& r, OptionalJacobian<1, 1> H) { + if (H) + *H = I_1x1; + Vector1 v; + v << r.theta(); + return v; +} +/* ************************************************************************* */ +Matrix2 Rot2::matrix() const { + Matrix2 rvalue_; + rvalue_ << c_, -s_, s_, c_; + return rvalue_; } /* ************************************************************************* */ -Matrix Rot2::transpose() const { - return (Matrix(2, 2) << c_, s_, -s_, c_).finished(); +Matrix2 Rot2::transpose() const { + Matrix2 rvalue_; + rvalue_ << c_, s_, -s_, c_; + return rvalue_; } /* ************************************************************************* */ // see doc/math.lyx, SO(2) section -Point2 Rot2::rotate(const Point2& p, boost::optional H1, - boost::optional H2) const { +Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1, + OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Matrix(2, 1) << -q.y(), q.x()).finished(); + if (H1) *H1 << -q.y(), q.x(); if (H2) *H2 = matrix(); return q; } @@ -86,21 +104,23 @@ Point2 Rot2::rotate(const Point2& p, boost::optional H1, /* ************************************************************************* */ // see doc/math.lyx, SO(2) section Point2 Rot2::unrotate(const Point2& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const { const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y()); - if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished(); // R_{pi/2}q + if (H1) *H1 << q.y(), -q.x(); if (H2) *H2 = transpose(); return q; } /* ************************************************************************* */ -Rot2 Rot2::relativeBearing(const Point2& d, boost::optional H) { +Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) { double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2); if(fabs(n) > 1e-5) { - if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished(); + if (H) { + *H << -y / d2, x / d2; + } return Rot2::fromCosSin(x / n, y / n); } else { - if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished(); + if (H) *H << 0.0, 0.0; return Rot2(); } } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index af9148fd3d..95f0256223 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) { @@ -87,7 +73,7 @@ namespace gtsam { * @param H optional reference for Jacobian * @return 2D rotation \f$ \in SO(2) \f$ */ - static Rot2 relativeBearing(const Point2& d, boost::optional H = + static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H = boost::none); /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */ @@ -111,68 +97,48 @@ namespace gtsam { inline static Rot2 identity() { return Rot2(); } /** The inverse rotation - negative angle */ - Rot2 inverse() const { - return Rot2(c_, -s_); - } - - /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = eye(1); - if (H2) *H2 = eye(1); - return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); - } + Rot2 inverse() const { return Rot2(c_, -s_);} /** Compose - make a new rotation by adding angles */ Rot2 operator*(const Rot2& R) const { return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); } - /** Between using the default implementation */ - inline Rot2 between(const Rot2& R, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = -eye(1); - if (H2) *H2 = eye(1); - return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_); - } - /// @} - /// @name Manifold + /// @name Lie Group /// @{ - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { - return dimension; - } + /// Exponential map at identity - create a rotation from canonical coordinates + static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none); - /// Dimensionality of the tangent space, DOF = 1 - inline size_t dim() const { - return dimension; - } + /// Log map at identity - return the canonical coordinates of this rotation + static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none); - /// Updates a with tangent space delta - inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); } + /** Calculate Adjoint map */ + Matrix1 AdjointMap() const { return I_1x1; } - /// Returns inverse retraction - inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); } - - /// @} - /// @name Lie Group - /// @{ - - ///Exponential map at identity - create a rotation from canonical coordinates - static Rot2 Expmap(const Vector& v) { - if (zero(v)) - return (Rot2()); - else - return Rot2::fromAngle(v(0)); + /// Left-trivialized derivative of the exponential map + static Matrix ExpmapDerivative(const Vector& v) { + return ones(1); } - ///Log map at identity - return the canonical coordinates of this rotation - static inline Vector Logmap(const Rot2& r) { - return (Vector(1) << r.theta()).finished(); + /// Left-trivialized derivative inverse of the exponential map + static Matrix LogmapDerivative(const Vector& v) { + return ones(1); } + // Chart at origin simply uses exponential map and its inverse + struct ChartAtOrigin { + static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) { + return Expmap(v, H); + } + static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) { + return Logmap(r, H); + } + }; + + using LieGroup::inverse; // version with derivative + /// @} /// @name Group Action on Point2 /// @{ @@ -180,8 +146,8 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point2 rotate(const Point2& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /** syntactic sugar for rotate */ inline Point2 operator*(const Point2& p) const { @@ -191,8 +157,8 @@ namespace gtsam { /** * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ */ - Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none, + OptionalJacobian<2, 2> H2 = boost::none) const; /// @} /// @name Standard Interface @@ -225,10 +191,10 @@ namespace gtsam { } /** return 2*2 rotation matrix */ - Matrix matrix() const; + Matrix2 matrix() const; /** return 2*2 transpose (inverse) rotation matrix */ - Matrix transpose() const; + Matrix2 transpose() const; private: /** Serialization function */ @@ -241,20 +207,7 @@ namespace gtsam { }; - // Define GTSAM traits - namespace traits { - - template<> - struct GTSAM_EXPORT is_group : 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 {}; - } } // gtsam diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 846e0e070c..ab44716c8b 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -27,8 +27,6 @@ using namespace std; namespace gtsam { -static const Matrix3 I3 = Matrix3::Identity(); - /* ************************************************************************* */ void Rot3::print(const std::string& s) const { gtsam::print((Matrix)matrix(), s); @@ -54,7 +52,7 @@ Rot3 Rot3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w) { +Rot3 Rot3::rodriguez(const Vector3& w) { double t = w.norm(); if (t < 1e-10) return Rot3(); return rodriguez(w/t, t); @@ -72,23 +70,21 @@ Point3 Rot3::operator*(const Point3& p) const { /* ************************************************************************* */ Unit3 Rot3::rotate(const Unit3& p, - boost::optional HR, boost::optional Hp) const { - Unit3 q = Unit3(rotate(p.point3(Hp))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix() * (*Hp); - if (HR) - (*HR) = -q.basis().transpose() * matrix() * p.skew(); + OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { + Matrix32 Dp; + Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0))); + if (Hp) *Hp = q.basis().transpose() * matrix() * Dp; + if (HR) *HR = -q.basis().transpose() * matrix() * p.skew(); return q; } /* ************************************************************************* */ Unit3 Rot3::unrotate(const Unit3& p, - boost::optional HR, boost::optional Hp) const { - Unit3 q = Unit3(unrotate(p.point3(Hp))); - if (Hp) - (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); - if (HR) - (*HR) = q.basis().transpose() * q.skew(); + OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const { + Matrix32 Dp; + Unit3 q = Unit3(unrotate(p.point3(Dp))); + if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp; + if (HR) *HR = q.basis().transpose() * q.skew(); return q; } @@ -99,8 +95,8 @@ Unit3 Rot3::operator*(const Unit3& p) const { /* ************************************************************************* */ // see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const { +Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1, + OptionalJacobian<3,3> H2) const { const Matrix3& Rt = transpose(); Point3 q(Rt * p.vector()); // q = Rt*p const double wx = q.x(), wy = q.y(), wz = q.z(); @@ -111,48 +107,6 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional H1, return q; } -/* ************************************************************************* */ -// see doc/math.lyx, SO(3) section -Point3 Rot3::unrotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Matrix3& Rt = transpose(); - Point3 q(Rt * p.vector()); // q = Rt*p - const double wx = q.x(), wy = q.y(), wz = q.z(); - if (H1) { - H1->resize(3,3); - *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; - } - if (H2) - *H2 = Rt; - return q; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2; - return res; -} - -/* ************************************************************************* */ -/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) -Matrix3 Rot3::dexpInvL(const Vector3& v) { - if(zero(v)) return eye(3); - Matrix x = skewSymmetric(v); - Matrix x2 = x*x; - double theta = v.norm(), vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - Matrix res = eye(3) + 0.5*x - s2*x2; - return res; -} - - /* ************************************************************************* */ Point3 Rot3::column(int index) const{ if(index == 3) @@ -167,7 +121,7 @@ Point3 Rot3::column(int index) const{ /* ************************************************************************* */ Vector3 Rot3::xyz() const { - Matrix I;Vector3 q; + Matrix3 I;Vector3 q; boost::tie(I,q)=RQ(matrix()); return q; } @@ -195,36 +149,57 @@ Vector Rot3::quaternion() const { } /* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jr; - if (normx < 10e-8){ - Jr = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X + - ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian - } - return Jr; -} - -/* ************************************************************************* */ -Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x) { - // x is the axis-angle representation (exponential coordinates) for a rotation - double normx = norm_2(x); // rotation angle - Matrix3 Jrinv; - - if (normx < 10e-8){ - Jrinv = Matrix3::Identity(); - } - else{ - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - Jrinv = Matrix3::Identity() + - 0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X; - } - return Jrinv; +Matrix3 Rot3::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::LogmapDerivative(const Vector3& x) { + if(zero(x)) return I_3x3; + double theta = x.norm(); +#ifdef DUY_VERSION + /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) + Matrix3 X = skewSymmetric(x); + Matrix3 X2 = X*X; + double vi = theta/2.0; + double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); + return I_3x3 + 0.5*X - s2*X2; +#else // Luca's version + /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in + * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. + * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega + * where Jrinv = LogmapDerivative(omega); + * This maps a perturbation on the manifold (expmap(omega)) + * to a perturbation in the tangent space (Jrinv * omega) + */ + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + return I_3x3 + 0.5 * X + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X + * X; +#endif } /* ************************************************************************* */ @@ -255,18 +230,12 @@ ostream &operator<<(ostream &os, const Rot3& R) { return os; } -/* ************************************************************************* */ -Point3 Rot3::unrotate(const Point3& p) const { - // Eigen expression - return Point3(transpose()*p.vector()); // q = Rt*p -} - /* ************************************************************************* */ Rot3 Rot3::slerp(double t, const Rot3& other) const { // amazingly simple in GTSAM :-) assert(t>=0 && t<=1); - Vector3 omega = localCoordinates(other, Rot3::EXPMAP); - return retract(t * omega, Rot3::EXPMAP); + Vector3 omega = Logmap(between(other)); + return compose(Expmap(t * omega)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 80ecd6d817..6ae5cad84c 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: @@ -157,7 +151,7 @@ namespace gtsam { * @param theta rotation angle * @return incremental rotation matrix */ - static Rot3 rodriguez(const Vector& w, double theta); + static Rot3 rodriguez(const Vector3& w, double theta); /** * Rodriguez' formula to compute an incremental rotation matrix @@ -180,7 +174,7 @@ namespace gtsam { * @param v a vector of incremental roll,pitch,yaw * @return incremental rotation matrix */ - static Rot3 rodriguez(const Vector& v); + static Rot3 rodriguez(const Vector3& v); /** * Rodriguez' formula to compute an incremental rotation matrix @@ -190,7 +184,7 @@ namespace gtsam { * @return incremental rotation matrix */ static Rot3 rodriguez(double wx, double wy, double wz) - { return rodriguez((Vector(3) << wx, wy, wz).finished());} + { return rodriguez(Vector3(wx, wy, wz));} /// @} /// @name Testable @@ -211,23 +205,13 @@ namespace gtsam { return Rot3(); } - /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(boost::optional H1=boost::none) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, boost::optional H1, - boost::optional H2) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, boost::optional H1, - boost::optional H2) const; - /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; + Rot3 inverse() const { + return Rot3(Matrix3(transpose())); + } + /** * Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C * @param cRb rotation from B frame to C frame @@ -238,23 +222,10 @@ namespace gtsam { return cRb * (*this) * cRb.inverse(); } - /** - * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' - */ - Rot3 between(const Rot3& R2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - /// @} /// @name Manifold /// @{ - /// dimension of the variable - used to autodetect sizes - static size_t Dim() { return 3; } - - /// return dimensionality of tangent space, DOF = 3 - size_t dim() const { return 3; } - /** * The method retract() is used to map from the tangent space back to the manifold. * Its inverse, is localCoordinates(). For Lie groups, an obvious retraction is the @@ -268,20 +239,28 @@ 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; -#endif + Rot3 retractCayley(const Vector& omega) const { + return compose(CayleyChart::Retract(omega)); + } - /// 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; + /// Inverse of retractCayley + Vector3 localCayley(const Rot3& other) const { + return CayleyChart::Local(between(other)); + } - /// 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; +#endif /// @} /// @name Lie Group @@ -291,32 +270,33 @@ namespace gtsam { * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodriguez' formula */ - static Rot3 Expmap(const Vector& v) { - if(zero(v)) return Rot3(); - else return rodriguez(v); + static Rot3 Expmap(const Vector& v, OptionalJacobian<3,3> H = boost::none) { + if(H) *H = Rot3::ExpmapDerivative(v); + if (zero(v)) return Rot3(); else return rodriguez(v); } /** - * Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z] \f$ of this rotation + * Log map at identity - returns the canonical coordinates + * \f$ [R_x,R_y,R_z] \f$ of this rotation */ - static Vector3 Logmap(const Rot3& R); + static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none); - /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector3& v); + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& x); - /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector3& v); + /// Derivative of Logmap + static Matrix3 LogmapDerivative(const Vector3& x); - /** - * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x); + /** Calculate Adjoint map */ + Matrix3 AdjointMap() const { return matrix(); } - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); + // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE + struct ChartAtOrigin { + static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none); + static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none); + }; + + using LieGroup::inverse; // version with derivative /// @} /// @name Group Action on Point3 @@ -325,34 +305,27 @@ namespace gtsam { /** * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$ */ - Point3 rotate(const Point3& p, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2 = boost::none) const; /// rotate point from rotated coordinate frame to world = R*p Point3 operator*(const Point3& p) const; /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const; - - /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ - Point3 unrotate(const Point3& p, boost::optional H1, - boost::optional H2) const; + Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none, + OptionalJacobian<3,3> H2=boost::none) const; /// @} /// @name Group Action on Unit3 /// @{ /// rotate 3D direction from rotated coordinate frame to world frame - Unit3 rotate(const Unit3& p, boost::optional HR = boost::none, - boost::optional Hp = boost::none) const; + Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, + OptionalJacobian<2,2> Hp = boost::none) const; /// unrotate 3D direction from world frame to rotated coordinate frame - Unit3 unrotate(const Unit3& p, boost::optional HR = boost::none, - boost::optional Hp = boost::none) const; + Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none, + OptionalJacobian<2,2> Hp = boost::none) const; /// rotate 3D direction from rotated coordinate frame to world frame Unit3 operator*(const Unit3& p) const; @@ -485,20 +458,7 @@ namespace gtsam { */ GTSAM_EXPORT std::pair 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 2c446c6d75..5e6b08fe2a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -30,10 +30,8 @@ using namespace std; namespace gtsam { -static const Matrix3 I3 = Matrix3::Identity(); - /* ************************************************************************* */ -Rot3::Rot3() : rot_(Matrix3::Identity()) {} +Rot3::Rot3() : rot_(I_3x3) {} /* ************************************************************************* */ Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { @@ -112,7 +110,7 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { } /* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w, double theta) { +Rot3 Rot3::rodriguez(const Vector3& w, double theta) { // get components of axis \omega double wx = w(0), wy=w(1), wz=w(2); double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; @@ -134,27 +132,6 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { -swy + C02, swx + C12, c + C22); } -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2) const { - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; -} - /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(Matrix3(rot_*R2.rot_)); @@ -166,24 +143,9 @@ Matrix3 Rot3::transpose() const { return rot_.transpose(); } -/* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 = -rot_; - return Rot3(Matrix3(transpose())); -} - -/* ************************************************************************* */ -Rot3 Rot3::between (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I3; - Matrix3 R12 = transpose()*R2.rot_; - return Rot3(R12); -} - /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1 || H2) { if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = rot_; @@ -193,7 +155,7 @@ Point3 Rot3::rotate(const Point3& p, /* ************************************************************************* */ // Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R) { +Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { static const double PI = boost::math::constants::pi(); @@ -201,6 +163,8 @@ Vector3 Rot3::Logmap(const Rot3& R) { // Get trace(R) double tr = rot.trace(); + Vector3 thetaR; + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // we do something special if (std::abs(tr+1.0) < 1e-10) { @@ -211,7 +175,7 @@ Vector3 Rot3::Logmap(const Rot3& R) { return (PI / sqrt(2.0+2.0*rot(1,1))) * Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*rot(0,0))) * + thetaR = (PI / sqrt(2.0+2.0*rot(0,0))) * Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); } else { double magnitude; @@ -224,68 +188,60 @@ Vector3 Rot3::Logmap(const Rot3& R) { // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) magnitude = 0.5 - tr_3*tr_3/12.0; } - return magnitude*Vector3( + thetaR = magnitude*Vector3( rot(2,1)-rot(1,2), rot(0,2)-rot(2,0), rot(1,0)-rot(0,1)); } + + if(H) *H = Rot3::LogmapDerivative(thetaR); + return thetaR; } /* ************************************************************************* */ -Rot3 Rot3::retractCayley(const Vector& omega) const { +Rot3 Rot3::CayleyChart::Retract(const Vector3& omega, OptionalJacobian<3,3> H) { + if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative"); const double x = omega(0), y = omega(1), z = omega(2); const double x2 = x * x, y2 = y * y, z2 = z * z; const double xy = x * y, xz = x * z, yz = y * z; const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; - return (*this) - * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, + return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); } /* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return (*this)*Expmap(omega); - } else if(mode == Rot3::CAYLEY) { - return retractCayley(omega); - } else if(mode == Rot3::SLOW_CAYLEY) { - Matrix Omega = skewSymmetric(omega); - return (*this)*CayleyFixed<3>(-Omega/2); - } else { - assert(false); - exit(1); - } +Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) { + if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative"); + // Create a fixed-size matrix + Matrix3 A = R.matrix(); + // Mathematica closed form optimization (procrastination?) gone wild: + const double a = A(0, 0), b = A(0, 1), c = A(0, 2); + const double d = A(1, 0), e = A(1, 1), f = A(1, 2); + const double g = A(2, 0), h = A(2, 1), i = A(2, 2); + const double di = d * i, ce = c * e, cd = c * d, fg = f * g; + const double M = 1 + e - f * h + i + e * i; + const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); } /* ************************************************************************* */ -Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return Logmap(between(T)); - } else if(mode == Rot3::CAYLEY) { - // Create a fixed-size matrix - Matrix3 A = rot_.transpose() * T.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a=A(0,0),b=A(0,1),c=A(0,2); - const double d=A(1,0),e=A(1,1),f=A(1,2); - const double g=A(2,0),h=A(2,1),i=A(2,2); - const double di = d*i, ce = c*e, cd = c*d, fg=f*g; - const double M = 1 + e - f*h + i + e*i; - const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = a * f - cd + f; - const double y = b * f - ce - c; - const double z = fg - di - d; - return K * Vector3(x, y, z); - } else if(mode == Rot3::SLOW_CAYLEY) { - // Create a fixed-size matrix - Matrix3 A(between(T).matrix()); - // using templated version of Cayley - Matrix3 Omega = CayleyFixed<3>(A); - return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); - } else { - assert(false); - exit(1); - } +Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Expmap(omega, H); + if (mode == Rot3::CAYLEY) return CayleyChart::Retract(omega, H); + else throw std::runtime_error("Rot3::Retract: unknown mode"); +} + +/* ************************************************************************* */ +Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Logmap(R, H); + if (mode == Rot3::CAYLEY) return CayleyChart::Local(R, H); + else throw std::runtime_error("Rot3::Local: unknown mode"); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 3f21c2a80c..d3b22430fe 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -80,25 +80,13 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::rodriguez(const Vector& w, double theta) { - return Quaternion(Eigen::AngleAxisd(theta, w)); } - - /* ************************************************************************* */ - Rot3 Rot3::compose(const Rot3& R2) const { - return Rot3(quaternion_ * R2.quaternion_); - } - - /* ************************************************************************* */ - Rot3 Rot3::compose(const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return Rot3(quaternion_ * R2.quaternion_); + Rot3 Rot3::rodriguez(const Vector3& w, double theta) { + return QuaternionChart::Expmap(theta,w); } /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = R2.transpose(); if (H2) *H2 = I3; return Rot3(quaternion_ * R2.quaternion_); @@ -110,7 +98,7 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::inverse(boost::optional H1) const { + Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { if (H1) *H1 = -matrix(); return Rot3(quaternion_.inverse()); } @@ -125,15 +113,15 @@ namespace gtsam { /* ************************************************************************* */ Rot3 Rot3::between(const Rot3& R2, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { if (H1) *H1 = -(R2.transpose()*matrix()); if (H2) *H2 = I3; - return between_default(*this, R2); + return inverse() * R2; } /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { Matrix R = matrix(); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H2) *H2 = R; @@ -142,31 +130,9 @@ namespace gtsam { } /* ************************************************************************* */ - Vector3 Rot3::Logmap(const Rot3& R) { - using std::acos; - using std::sqrt; - static const double twoPi = 2.0 * M_PI, - // define these compile time constants to avoid std::abs: - NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; - - const Quaternion& q = R.quaternion_; - const double qw = q.w(); - if (qw > NearlyOne) { - // Taylor expansion of (angle / s) at 1 - return (2 - 2 * (qw - 1) / 3) * q.vec(); - } else if (qw < NearlyNegativeOne) { - // Angle is zero, return zero vector - return Vector3::Zero(); - } else { - // Normal, away from zero case - double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); - // Important: convert to [-pi,pi] to keep error continuous - if (angle > M_PI) - angle -= twoPi; - else if (angle < -M_PI) - angle += twoPi; - return (angle / s) * q.vec(); - } + Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { + if(H) *H = Rot3::LogmapDerivative(thetaR); + return QuaternionChart::Logmap(R.quaternion_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp new file mode 100644 index 0000000000..08ae31945e --- /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 0000000000..bed2f12122 --- /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/SimpleCamera.cpp b/gtsam/geometry/SimpleCamera.cpp index cab8718749..d92c5bdd72 100644 --- a/gtsam/geometry/SimpleCamera.cpp +++ b/gtsam/geometry/SimpleCamera.cpp @@ -21,27 +21,27 @@ namespace gtsam { - SimpleCamera simpleCamera(const Matrix& P) { + SimpleCamera simpleCamera(const Matrix34& P) { // P = [A|a] = s K cRw [I|-T], with s the unknown scale - Matrix A = P.topLeftCorner(3, 3); - Vector a = P.col(3); + Matrix3 A = P.topLeftCorner(3, 3); + Vector3 a = P.col(3); // do RQ decomposition to get s*K and cRw angles - Matrix sK; - Vector xyz; + Matrix3 sK; + Vector3 xyz; boost::tie(sK, xyz) = RQ(A); // Recover scale factor s and K double s = sK(2, 2); - Matrix K = sK / s; + Matrix3 K = sK / s; // Recover cRw itself, and its inverse Rot3 cRw = Rot3::RzRyRx(xyz); Rot3 wRc = cRw.inverse(); // Now, recover T from a = - s K cRw T = - A T - Vector T = -(A.inverse() * a); + Vector3 T = -(A.inverse() * a); return SimpleCamera(Pose3(wRc, T), Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2))); } diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 557654d2df..c6d33bcb3f 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -27,5 +27,5 @@ namespace gtsam { typedef PinholeCamera SimpleCamera; /// Recover camera from 3*4 camera matrix - GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix& P); + GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P); } diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index b9e03c01dd..9170f82825 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -30,8 +30,8 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, - boost::optional H1, boost::optional H2, - boost::optional H3) const { + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2, + OptionalJacobian<3,6> H3) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); @@ -58,28 +58,28 @@ namespace gtsam { if (H1 || H2) { #ifdef STEREOCAMERA_CHAIN_RULE // just implement chain rule - Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian + Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian if (H1) *H1 = D_project_point*(*H1); if (H2) *H2 = D_project_point*(*H2); #else // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; - *H1 = (Matrix(3, 6) << - uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, + *H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR, - fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v - ).finished(); + fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v; } if (H2) { - const Matrix R(leftCamPose_.rotation().matrix()); - *H2 = d * (Matrix(3, 3) << - fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, - fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, - fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v - ).finished(); + const Matrix3 R(leftCamPose_.rotation().matrix()); + *H2 << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, + fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, + fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v; + *H2 << d * (*H2); } #endif + if (H3) + // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet + H3->setZero(); } // finally translate @@ -87,15 +87,15 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { + Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { double d = 1.0 / P.z(), d2 = d*d; const Cal3_S2Stereo& K = *K_; double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); - return (Matrix(3, 3) << - f_x*d, 0.0, -d2*f_x* P.x(), + Matrix3 m; + m << f_x*d, 0.0, -d2*f_x* P.x(), f_x*d, 0.0, -d2*f_x*(P.x() - b), - 0.0, f_y*d, -d2*f_y* P.y() - ).finished(); + 0.0, f_y*d, -d2*f_y* P.y(); + return m; } } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 60ea7693de..49abcf36b7 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -44,6 +44,8 @@ class GTSAM_EXPORT StereoCamera { public: + enum { dimension = 6 }; + /// @name Standard Constructors /// @{ @@ -90,14 +92,8 @@ class GTSAM_EXPORT StereoCamera { } /// Local coordinates of manifold neighborhood around current value - inline Vector localCoordinates(const StereoCamera& t2) const { - return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_)); - } - - Pose3 between(const StereoCamera &camera, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { - return leftCamPose_.between(camera.pose(), H1, H2); + inline Vector6 localCoordinates(const StereoCamera& t2) const { + return leftCamPose_.localCoordinates(t2.leftCamPose_); } /// @} @@ -119,9 +115,9 @@ class GTSAM_EXPORT StereoCamera { * @param H3 IGNORED (for calibration) */ StereoPoint2 project(const Point3& point, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const; + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none, + OptionalJacobian<3, 6> H3 = boost::none) const; /** * @@ -139,7 +135,7 @@ class GTSAM_EXPORT StereoCamera { private: /** utility functions */ - Matrix Dproject_to_stereo_camera1(const Point3& P) const; + Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; template @@ -150,22 +146,7 @@ class GTSAM_EXPORT StereoCamera { }; -// 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 694bf525bd..2ec745fd8a 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 5e13129cc6..611cf7cde3 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -39,14 +39,13 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Unit3 Unit3::FromPoint3(const Point3& point, boost::optional H) { +Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { Unit3 direction(point); if (H) { // 3*3 Derivative of representation with respect to point is 3*3: - Matrix D_p_point; + Matrix3 D_p_point; point.normalize(D_p_point); // TODO, this calculates norm a second time :-( // Calculate the 2*3 Jacobian - H->resize(2, 3); *H << direction.basis().transpose() * D_p_point; } return direction; @@ -67,7 +66,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -const Unit3::Matrix32& Unit3::basis() const { +const Matrix32& Unit3::basis() const { // Return cached version if exists if (B_) @@ -92,7 +91,7 @@ const Unit3::Matrix32& Unit3::basis() const { b2 = b2 / b2.norm(); // Create the basis matrix - B_.reset(Unit3::Matrix32()); + B_.reset(Matrix32()); (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); return *B_; } @@ -104,38 +103,39 @@ void Unit3::print(const std::string& s) const { } /* ************************************************************************* */ -Matrix Unit3::skew() const { +Matrix3 Unit3::skew() const { return skewSymmetric(p_.x(), p_.y(), p_.z()); } /* ************************************************************************* */ -Vector Unit3::error(const Unit3& q, boost::optional H) const { +Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 - Matrix Bt = basis().transpose(); - Vector xi = Bt * q.p_.vector(); + Matrix23 Bt = basis().transpose(); + Vector2 xi = Bt * q.p_.vector(); if (H) *H = Bt * q.basis(); return xi; } /* ************************************************************************* */ -double Unit3::distance(const Unit3& q, boost::optional H) const { - Vector xi = error(q, H); +double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { + Matrix2 H_; + Vector2 xi = error(q, H_); double theta = xi.norm(); if (H) - *H = (xi.transpose() / theta) * (*H); + *H = (xi.transpose() / theta) * H_; return theta; } /* ************************************************************************* */ -Unit3 Unit3::retract(const Vector& v) const { +Unit3 Unit3::retract(const Vector2& v) const { // Get the vector form of the point and the basis matrix - Vector p = Point3::Logmap(p_); - Matrix B = basis(); + Vector3 p = p_.vector(); + Matrix32 B = basis(); // Compute the 3D xi_hat vector - Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); + Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); double xi_hat_norm = xi_hat.norm(); @@ -147,28 +147,27 @@ Unit3 Unit3::retract(const Vector& v) const { return Unit3(-point3()); } - Vector exp_p_xi_hat = cos(xi_hat_norm) * p + Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); return Unit3(exp_p_xi_hat); } /* ************************************************************************* */ -Vector Unit3::localCoordinates(const Unit3& y) const { +Vector2 Unit3::localCoordinates(const Unit3& y) const { - Vector p = Point3::Logmap(p_); - Vector q = Point3::Logmap(y.p_); + Vector3 p = p_.vector(), q = y.p_.vector(); double dot = p.dot(q); // Check for special cases if (std::abs(dot - 1.0) < 1e-16) return Vector2(0, 0); else if (std::abs(dot + 1.0) < 1e-16) - return (Vector(2) << M_PI, 0).finished(); + return Vector2(M_PI, 0); else { // no special case double theta = acos(dot); - Vector result_hat = (theta / sin(theta)) * (q - p * dot); + Vector3 result_hat = (theta / sin(theta)) * (q - p * dot); return basis().transpose() * result_hat; } } diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index d04e57d4b6..8784d0eb8e 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -20,6 +20,7 @@ #pragma once +#include #include #include #include @@ -28,17 +29,17 @@ namespace gtsam { /// Represents a 3D point on a unit sphere. -class GTSAM_EXPORT Unit3{ +class GTSAM_EXPORT Unit3 { private: - typedef Eigen::Matrix Matrix32; - Point3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis public: + enum { dimension = 2 }; + /// @name Constructors /// @{ @@ -52,6 +53,11 @@ class GTSAM_EXPORT Unit3{ p_(p / p.norm()) { } + /// Construct from a vector3 + explicit Unit3(const Vector3& p) : + p_(p / p.norm()) { + } + /// Construct from x,y,z Unit3(double x, double y, double z) : p_(x, y, z) { @@ -59,7 +65,7 @@ class GTSAM_EXPORT Unit3{ } /// Named constructor from Point3 with optional Jacobian - static Unit3 FromPoint3(const Point3& point, boost::optional H = + static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H = boost::none); /// Random direction, using boost::uniform_on_sphere @@ -90,10 +96,10 @@ class GTSAM_EXPORT Unit3{ const Matrix32& basis() const; /// Return skew-symmetric associated with 3D point on unit sphere - Matrix skew() const; + Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(boost::optional H = boost::none) const { + const Point3& point3(OptionalJacobian<3,2> H = boost::none) const { if (H) *H = basis(); return p_; @@ -105,12 +111,12 @@ class GTSAM_EXPORT Unit3{ } /// Signed, vector-valued error between two directions - Vector error(const Unit3& q, - boost::optional H = boost::none) const; + Vector2 error(const Unit3& q, + OptionalJacobian<2,2> H = boost::none) const; /// Distance between two directions double distance(const Unit3& q, - boost::optional H = boost::none) const; + OptionalJacobian<1,2> H = boost::none) const; /// @} @@ -133,10 +139,10 @@ class GTSAM_EXPORT Unit3{ }; /// The retract function - Unit3 retract(const Vector& v) const; + Unit3 retract(const Vector2& v) const; /// The local coordinates function - Vector localCoordinates(const Unit3& s) const; + Vector2 localCoordinates(const Unit3& s) const; /// @} @@ -144,13 +150,18 @@ class GTSAM_EXPORT Unit3{ /// @name Advanced Interface /// @{ - /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_NVP(p_); - ar & BOOST_SERIALIZATION_NVP(B_); + // homebrew serialize Eigen Matrix + ar & boost::serialization::make_nvp("B11", (*B_)(0,0)); + ar & boost::serialization::make_nvp("B12", (*B_)(0,1)); + ar & boost::serialization::make_nvp("B21", (*B_)(1,0)); + ar & boost::serialization::make_nvp("B22", (*B_)(1,1)); + ar & boost::serialization::make_nvp("B31", (*B_)(2,0)); + ar & boost::serialization::make_nvp("B32", (*B_)(2,1)); } /// @} @@ -158,24 +169,7 @@ class GTSAM_EXPORT Unit3{ }; // 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/testCal3_S2.cpp b/gtsam/geometry/tests/testCal3_S2.cpp index 86c0b7e336..aa06a2e292 100644 --- a/gtsam/geometry/tests/testCal3_S2.cpp +++ b/gtsam/geometry/tests/testCal3_S2.cpp @@ -60,7 +60,7 @@ TEST( Cal3_S2, calibrate_homogeneous) { Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); } TEST( Cal3_S2, Duncalibrate1) { - Matrix computed; K.uncalibrate(p, computed, boost::none); + Matrix25 computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p); CHECK(assert_equal(numerical,computed,1e-8)); } diff --git a/gtsam/geometry/tests/testCalibratedCamera.cpp b/gtsam/geometry/tests/testCalibratedCamera.cpp index 5aeee03d4e..6a990e08e7 100644 --- a/gtsam/geometry/tests/testCalibratedCamera.cpp +++ b/gtsam/geometry/tests/testCalibratedCamera.cpp @@ -15,12 +15,14 @@ * @brief test CalibratedCamera class */ -#include - -#include +#include +#include #include #include -#include + +#include + +#include using namespace std; using namespace gtsam; diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp new file mode 100644 index 0000000000..a15d7e2c25 --- /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/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index ddeae2b7d6..6cb84ec853 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -15,29 +15,28 @@ * @brief test PinholeCamera class */ -#include -#include - #include #include #include #include #include +#include + +#include +#include using namespace std; using namespace gtsam; +typedef PinholeCamera Camera; + static const Cal3_S2 K(625, 625, 0, 0, 0); -static const Pose3 pose1((Matrix)(Matrix(3,3) << - 1., 0., 0., - 0.,-1., 0., - 0., 0.,-1. - ).finished(), - Point3(0,0,0.5)); +static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5)); +static const Camera camera(pose, K); -typedef PinholeCamera Camera; -static const Camera camera(pose1, K); +static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5)); +static const Camera camera1(pose1, K); static const Point3 point1(-0.08,-0.08, 0.0); static const Point3 point2(-0.08, 0.08, 0.0); @@ -52,8 +51,8 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) { - CHECK(assert_equal( camera.calibration(), K)); - CHECK(assert_equal( camera.pose(), pose1)); + EXPECT(assert_equal( camera.calibration(), K)); + EXPECT(assert_equal( camera.pose(), pose)); } /* ************************************************************************* */ @@ -67,7 +66,7 @@ TEST( PinholeCamera, level2) Point3 x(1,0,0),y(0,0,-1),z(0,1,0); Rot3 wRc(x,y,z); Pose3 expected(wRc,Point3(0.4,0.3,0.1)); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); } /* ************************************************************************* */ @@ -80,72 +79,72 @@ TEST( PinholeCamera, lookat) // expected Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0); Pose3 expected(Rot3(xc,yc,zc),C); - CHECK(assert_equal( camera.pose(), expected)); + EXPECT(assert_equal( camera.pose(), expected)); Point3 C2(30.0,0.0,10.0); Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0)); Matrix R = camera2.pose().rotation().matrix(); Matrix I = trans(R)*R; - CHECK(assert_equal(I, eye(3))); + EXPECT(assert_equal(I, eye(3))); } /* ************************************************************************* */ TEST( PinholeCamera, project) { - CHECK(assert_equal( camera.project(point1), Point2(-100, 100) )); - CHECK(assert_equal( camera.project(point2), Point2(-100, -100) )); - CHECK(assert_equal( camera.project(point3), Point2( 100, -100) )); - CHECK(assert_equal( camera.project(point4), Point2( 100, 100) )); + EXPECT(assert_equal( camera.project(point1), Point2(-100, 100) )); + EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) )); + EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) )); + EXPECT(assert_equal( camera.project(point4), Point2( 100, 100) )); } /* ************************************************************************* */ TEST( PinholeCamera, backproject) { - CHECK(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); - CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); - CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); - CHECK(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); + EXPECT(assert_equal( camera.backproject(Point2(-100, 100), 0.5), point1)); + EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5), point2)); + EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5), point3)); + EXPECT(assert_equal( camera.backproject(Point2( 100, 100), 0.5), point4)); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity) { - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); - CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, 100)), point1_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)), point2_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)), point3_inf)); + EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, 100)), point4_inf)); } /* ************************************************************************* */ TEST( PinholeCamera, backproject2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); Point3 actual = camera.backproject(Point2(), 1.); Point3 expected(0., 1., 0.); pair x = camera.projectSafe(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x.first)); - CHECK(x.second); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x.first)); + EXPECT(x.second); } /* ************************************************************************* */ TEST( PinholeCamera, backprojectInfinity2) { Point3 origin; - Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down + Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down Camera camera(Pose3(rot, origin), K); Point3 actual = camera.backprojectPointAtInfinity(Point2()); Point3 expected(0., 1., 0.); Point2 x = camera.projectPointAtInfinity(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -159,8 +158,8 @@ TEST( PinholeCamera, backprojectInfinity3) Point3 expected(0., 0., 1.); Point2 x = camera.projectPointAtInfinity(expected); - CHECK(assert_equal(expected, actual)); - CHECK(assert_equal(Point2(), x)); + EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(Point2(), x)); } /* ************************************************************************* */ @@ -173,13 +172,13 @@ TEST( PinholeCamera, Dproject) { Matrix Dpose, Dpoint, Dcal; Point2 result = camera.project(point1, Dpose, Dpoint, Dcal); - Matrix numerical_pose = numericalDerivative31(project3, pose1, point1, K); - Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K); - Matrix numerical_cal = numericalDerivative33(project3, pose1, point1, K); - CHECK(assert_equal(Point2(-100, 100), result)); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(project3, pose, point1, K); + Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K); + Matrix numerical_cal = numericalDerivative33(project3, pose, point1, K); + EXPECT(assert_equal(Point2(-100, 100), result)); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -190,21 +189,21 @@ static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const C TEST( PinholeCamera, Dproject_Infinity) { Matrix Dpose, Dpoint, Dcal; - Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera + Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1 // test Projection Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); Point2 expected(-5.0, 5.0); - CHECK(assert_equal(actual, expected, 1e-7)); + EXPECT(assert_equal(actual, expected, 1e-7)); // test Jacobians - Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K); - Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K); - Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters - Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K); - CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); - CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7)); - CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose, point3D, K); + Matrix Hexpected2 = numericalDerivative32(projectInfinity3, pose, point3D, K); + Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose, point3D, K); + EXPECT(assert_equal(numerical_pose, Dpose, 1e-7)); + EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7)); + EXPECT(assert_equal(numerical_cal, Dcal, 1e-7)); } /* ************************************************************************* */ @@ -217,11 +216,80 @@ TEST( PinholeCamera, Dproject2) { Matrix Dcamera, Dpoint; Point2 result = camera.project2(point1, Dcamera, Dpoint); - Matrix numerical_camera = numericalDerivative21(project4, camera, point1); - Matrix numerical_point = numericalDerivative22(project4, camera, point1); - CHECK(assert_equal(result, Point2(-100, 100) )); - CHECK(assert_equal(numerical_camera, Dcamera, 1e-7)); - CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); + Matrix Hexpected1 = numericalDerivative21(project4, camera, point1); + Matrix Hexpected2 = numericalDerivative22(project4, camera, point1); + EXPECT(assert_equal(result, Point2(-100, 100) )); + EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7)); + EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7)); +} + +/* ************************************************************************* */ +static double range0(const Camera& camera, const Point3& point) { + return camera.range(point); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range0) { + Matrix D1; Matrix D2; + double result = camera.range(point1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range0, camera, point1); + Matrix Hexpected2 = numericalDerivative22(range0, camera, point1); + EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result, + 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static double range1(const Camera& camera, const Pose3& pose) { + return camera.range(pose); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range1) { + Matrix D1; Matrix D2; + double result = camera.range(pose1, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1); + Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +typedef PinholeCamera Camera2; +static const Cal3Bundler K2(625, 1e-3, 1e-3); +static const Camera2 camera2(pose1, K2); +static double range2(const Camera& camera, const Camera2& camera2) { + return camera.range(camera2); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range2) { + Matrix D1; Matrix D2; + double result = camera.range(camera2, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2); + Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); +} + +/* ************************************************************************* */ +static const CalibratedCamera camera3(pose1); +static double range3(const Camera& camera, const CalibratedCamera& camera3) { + return camera.range(camera3); +} + +/* ************************************************************************* */ +TEST( PinholeCamera, range3) { + Matrix D1; Matrix D2; + double result = camera.range(camera3, D1, D2); + Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3); + Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3); + EXPECT_DOUBLES_EQUAL(1, result, 1e-9); + EXPECT(assert_equal(Hexpected1, D1, 1e-7)); + EXPECT(assert_equal(Hexpected2, D2, 1e-7)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index ab40928abb..fce7955e97 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 18273b1821..5c6b32dcaa 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 b4de6eb7c5..fa6bd203cd 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; @@ -44,7 +49,7 @@ TEST(Pose2, constructors) { Pose2 origin; assert_equal(pose,origin); Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01)); - EXPECT(assert_equal(t,Pose2(t.matrix()))); + EXPECT(assert_equal(t,Pose2((Matrix)t.matrix()))); } /* ************************************************************************* */ @@ -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 @@ -519,7 +557,7 @@ TEST( Pose2, bearing ) expectedH1 = numericalDerivative21(bearing_proxy, x2, l3); EXPECT(assert_equal(expectedH1,actualH1)); expectedH2 = numericalDerivative22(bearing_proxy, x2, l3); - EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); // establish bearing is indeed 45 degrees even if rotated Rot2 actual34 = x3.bearing(l4, actualH1, actualH2); @@ -529,7 +567,7 @@ TEST( Pose2, bearing ) expectedH1 = numericalDerivative21(bearing_proxy, x3, l4); expectedH2 = numericalDerivative22(bearing_proxy, x3, l4); EXPECT(assert_equal(expectedH1,actualH1)); - EXPECT(assert_equal(expectedH1,actualH1)); + EXPECT(assert_equal(expectedH2,actualH2)); } /* ************************************************************************* */ @@ -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 52f721f416..d40344d354 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) { - 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))); + 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, localCoordinates_expmap) +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::EXPMAP); - EXPECT(assert_equal(d12, t1.localCoordinates(t2, Pose3::EXPMAP))); + Pose3 t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); } /* ************************************************************************* */ -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, 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)); } -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, 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)); } /* ************************************************************************* */ -Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) { +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 0000000000..7302754d7a --- /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 8a1f942d22..37054fd832 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; @@ -49,7 +49,9 @@ TEST( Rot2, unit) /* ************************************************************************* */ TEST( Rot2, transpose) { - CHECK(assert_equal(R.inverse().matrix(),R.transpose())); + Matrix expected = R.inverse().matrix(); + Matrix actual = R.transpose(); + CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ @@ -153,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 88accb90f8..96346e382e 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 @@ -37,20 +36,25 @@ GTSAM_CONCEPT_LIE_INST(Rot3) 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; -static const Matrix I3 = eye(3); + +//****************************************************************************** +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); } /* ************************************************************************* */ TEST( Rot3, constructor) { - Rot3 expected(I3); + Rot3 expected((Matrix)I_3x3); Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1); Rot3 actual(r1, r2, r3); CHECK(assert_equal(actual,expected)); @@ -95,7 +99,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); - Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J); + Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J); return R; } @@ -149,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))); } /* ************************************************************************* */ @@ -215,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 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)); +} - Matrix expectedJacobian = numericalDerivative11( - boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); - EXPECT(assert_equal(expectedJacobian, actualJacobian)); +/* ************************************************************************* */ +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)); } /* ************************************************************************* */ @@ -252,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))); @@ -359,7 +422,7 @@ TEST( Rot3, inverse ) Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); Rot3 I; - Matrix actualH; + Matrix3 actualH; Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); @@ -401,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 ) { @@ -482,7 +524,7 @@ TEST( Rot3, RQ) Vector actual; boost::tie(actualK, actual) = RQ(R.matrix()); Vector expected = Vector3(0.14715, 0.385821, 0.231671); - CHECK(assert_equal(I3,actualK)); + CHECK(assert_equal(I_3x3,actualK)); CHECK(assert_equal(expected,actual,1e-6)); // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] @@ -516,7 +558,7 @@ TEST( Rot3, expmapStability ) { w(2), 0.0, -w(0), -w(1), w(0), 0.0 ).finished(); Matrix W2 = W*W; - Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0 + Matrix Rmat = I_3x3 + (1.0-theta2/6.0 + theta2*theta2/120.0 - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; Rot3 expectedR( Rmat ); CHECK(assert_equal(expectedR, actualR, 1e-10)); @@ -575,10 +617,16 @@ 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); - EXPECT(assert_equal(I3, trans(Q)*Q)); + EXPECT(assert_equal((Matrix)I_3x3, trans(Q)*Q)); EXPECT(assert_equal(A, Cayley(Q))); } @@ -603,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 1313a3be59..12fb94bbc0 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 @@ -37,7 +30,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3) static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Point3 P(0.2, 0.7, -2.0); -static const Matrix I3 = eye(3); /* ************************************************************************* */ TEST(Rot3, manifold_cayley) @@ -47,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 0000000000..0fe6991165 --- /dev/null +++ b/gtsam/geometry/tests/testSO3.cpp @@ -0,0 +1,94 @@ +/* ---------------------------------------------------------------------------- + + * 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 , 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/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index a7e792cb83..bbc8be1ad1 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -19,6 +19,8 @@ #include #include #include +#include +#include #include #include #include @@ -41,6 +43,9 @@ static Point3 pt3(1.0, 2.0, 3.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); static Pose3 pose3(rt3, pt3); +static Unit3 unit3(1.0, 2.1, 3.4); +static EssentialMatrix ematrix(rt3, unit3); + static Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); 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); @@ -59,6 +64,9 @@ TEST (Serialization, text_geometry) { EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsObj(Rot2::fromDegrees(30.0))); + EXPECT(equalsObj(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsObj(EssentialMatrix(rt3, unit3))); + EXPECT(equalsObj(pt3)); EXPECT(equalsObj(rt3)); EXPECT(equalsObj(Pose3(rt3, pt3))); @@ -81,6 +89,9 @@ TEST (Serialization, xml_geometry) { EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsXML(Rot2::fromDegrees(30.0))); + EXPECT(equalsXML(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsXML(EssentialMatrix(rt3, unit3))); + EXPECT(equalsXML(pt3)); EXPECT(equalsXML(rt3)); EXPECT(equalsXML(Pose3(rt3, pt3))); @@ -102,6 +113,9 @@ TEST (Serialization, binary_geometry) { EXPECT(equalsBinary(Pose2(1.0, 2.0, 0.3))); EXPECT(equalsBinary(Rot2::fromDegrees(30.0))); + EXPECT(equalsBinary(Unit3(1.0, 2.1, 3.4))); + EXPECT(equalsBinary(EssentialMatrix(rt3, unit3))); + EXPECT(equalsBinary(pt3)); EXPECT(equalsBinary(rt3)); EXPECT(equalsBinary(Pose3(rt3, pt3))); diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index eebe2d60ab..c77509b913 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -74,7 +74,7 @@ TEST( StereoCamera, project) /* ************************************************************************* */ -static Pose3 camera1((Matrix)(Matrix(3,3) << +static Pose3 camPose((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -82,33 +82,41 @@ static Pose3 camera1((Matrix)(Matrix(3,3) << Point3(0,0,6.25)); static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); -static StereoCamera stereoCam(Pose3(), K); +static StereoCamera stereoCam(camPose, K); // point X Y Z in meters -static Point3 p(0, 0, 5); +static Point3 landmark(0, 0, 5); /* ************************************************************************* */ -static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); } -TEST( StereoCamera, Dproject_stereo_pose) -{ - Matrix expected = numericalDerivative21(project_,stereoCam, p); - Matrix actual; stereoCam.project(p, actual, boost::none); - CHECK(assert_equal(expected,actual,1e-7)); +static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_S2Stereo& K) { + return StereoCamera(pose, boost::make_shared(K)).project(point); } /* ************************************************************************* */ -TEST( StereoCamera, Dproject_stereo_point) +TEST( StereoCamera, Dproject) { - Matrix expected = numericalDerivative22(project_,stereoCam, p); - Matrix actual; stereoCam.project(p, boost::none, actual); - CHECK(assert_equal(expected,actual,1e-8)); + Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K); + Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none); + CHECK(assert_equal(expected1,actual1,1e-7)); + + Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K); + Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none); + CHECK(assert_equal(expected2,actual2,1e-7)); + + Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K); + Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3); +// CHECK(assert_equal(expected3,actual3,1e-8)); } +/* ************************************************************************* */ TEST( StereoCamera, backproject) { + Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam2(Pose3(), K2); + Point3 expected(1.2, 2.3, 4.5); - StereoPoint2 stereo_point = stereoCam.project(expected); - Point3 actual = stereoCam.backproject(stereo_point); + StereoPoint2 stereo_point = stereoCam2.project(expected); + Point3 actual = stereoCam2.backproject(stereo_point); CHECK(assert_equal(expected,actual,1e-8)); } diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index 8e504ba0ec..5496b8aacf 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/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 0bd553a402..f986cca1d9 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -262,25 +262,6 @@ TEST( triangulation, twoIdenticalPoses) { } */ -//****************************************************************************** -TEST( triangulation, TriangulationFactor ) { - // Create the factor with a measurement that is 3 pixels off in x - Key pointKey(1); - SharedNoiseModel model; - typedef TriangulationFactor<> Factor; - Factor factor(camera1, z1, model, pointKey); - - // Use the factor to calculate the Jacobians - Matrix HActual; - factor.evaluateError(landmark, HActual); - - Matrix HExpected = numericalDerivative11( - boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); - - // Verify the Jacobians are correct - CHECK(assert_equal(HExpected, HActual, 1e-3)); -} - //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index 0102477b3c..1d0c28ded5 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -108,20 +108,20 @@ TEST(Unit3, unrotate) { TEST(Unit3, error) { Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), // r = p.retract(Vector2(0.8, 0)); - EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8)); - EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5)); - EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8)); + EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5)); + EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5)); 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/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 9e15758011..4746895259 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -30,7 +30,7 @@ namespace gtsam { * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -Point3 triangulateDLT(const std::vector& projection_matrices, +Point3 triangulateDLT(const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { // number of cameras @@ -41,7 +41,7 @@ Point3 triangulateDLT(const std::vector& projection_matrices, for (size_t i = 0; i < m; i++) { size_t row = i * 2; - const Matrix& projection = projection_matrices.at(i); + const Matrix34& projection = projection_matrices.at(i); const Point2& p = measurements.at(i); // build system of equations diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index fabcc4c027..ce83f780b4 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -19,12 +19,10 @@ #pragma once -#include +#include +#include #include #include -#include - -#include namespace gtsam { @@ -53,7 +51,7 @@ class TriangulationCheiralityException: public std::runtime_error { * @return Triangulated Point3 */ GTSAM_EXPORT Point3 triangulateDLT( - const std::vector& projection_matrices, + const std::vector& projection_matrices, const std::vector& measurements, double rank_tol); /// @@ -189,12 +187,11 @@ Point3 triangulatePoint3(const std::vector& poses, throw(TriangulationUnderconstrainedException()); // construct projection matrices from poses & calibration - std::vector projection_matrices; + std::vector projection_matrices; BOOST_FOREACH(const Pose3& pose, poses) { projection_matrices.push_back( - sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4)); + sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0)); } - // Triangulate linearly Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); @@ -240,12 +237,11 @@ Point3 triangulatePoint3( // construct projection matrices from poses & calibration typedef PinholeCamera Camera; - std::vector projection_matrices; - BOOST_FOREACH(const Camera& camera, cameras) - projection_matrices.push_back( - camera.calibration().K() - * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4)); - + std::vector projection_matrices; + BOOST_FOREACH(const Camera& camera, cameras) { + Matrix P_ = (camera.pose().inverse().matrix()); + projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) ); + } Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); // The n refine using non-linear optimization diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 043c4caf64..adb1c0aad8 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.cpp b/gtsam/inference/LabeledSymbol.cpp index 7e573c13fd..b9e93ceb1f 100644 --- a/gtsam/inference/LabeledSymbol.cpp +++ b/gtsam/inference/LabeledSymbol.cpp @@ -17,12 +17,8 @@ #include -#include #include -#include -#include -#include -#include +#include #include @@ -111,23 +107,21 @@ bool LabeledSymbol::operator!=(gtsam::Key comp) const { } /* ************************************************************************* */ +static LabeledSymbol make(gtsam::Key key) { return LabeledSymbol(key);} + boost::function LabeledSymbol::TypeTest(unsigned char c) { - namespace bl = boost::lambda; - return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor(), bl::_1)) == c; + return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c; } -/* ************************************************************************* */ boost::function LabeledSymbol::LabelTest(unsigned char label) { - namespace bl = boost::lambda; - return bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor(), bl::_1)) == label; + return boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; } -/* ************************************************************************* */ boost::function LabeledSymbol::TypeLabelTest(unsigned char c, unsigned char label) { - namespace bl = boost::lambda; - return bl::bind(&LabeledSymbol::chr, bl::bind(bl::constructor(), bl::_1)) == c && - bl::bind(&LabeledSymbol::label, bl::bind(bl::constructor(), bl::_1)) == label; + return boost::bind(&LabeledSymbol::chr, boost::bind(make, _1)) == c && + boost::bind(&LabeledSymbol::label, boost::bind(make, _1)) == label; } +/* ************************************************************************* */ } // \namespace gtsam diff --git a/gtsam/inference/LabeledSymbol.h b/gtsam/inference/LabeledSymbol.h index 4b125988cc..23936afed7 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 24c8118415..e25590578c 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.cpp b/gtsam/inference/Symbol.cpp index 37a6d0897c..f8b37d4293 100644 --- a/gtsam/inference/Symbol.cpp +++ b/gtsam/inference/Symbol.cpp @@ -18,19 +18,8 @@ #include -#include #include -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wunused-variable" -#endif -#include -#include -#ifdef __GNUC__ -#pragma GCC diagnostic pop -#endif +#include #include #include @@ -71,10 +60,10 @@ Symbol::operator std::string() const { return str(boost::format("%c%d") % c_ % j_); } +static Symbol make(gtsam::Key key) { return Symbol(key);} + boost::function Symbol::ChrTest(unsigned char c) { - namespace bl = boost::lambda; - return bl::bind(&Symbol::chr, bl::bind(bl::constructor(), bl::_1)) - == c; + return bind(&Symbol::chr, bind(make, _1)) == c; } } // namespace gtsam diff --git a/gtsam/inference/Symbol.h b/gtsam/inference/Symbol.h index 6963905e01..4f9734639e 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 3985221d3b..bcaec6ee41 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 namespace gtsam { @@ -175,6 +177,11 @@ class GTSAM_EXPORT VariableIndex { /// @} }; -} +/// traits +template<> +struct traits : public Testable { +}; + +} //\ namespace gtsam #include diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index 9a16ca788e..fad7894362 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 @@ class VariableSlots : public FastMap > { /// @} }; +/// traits +template<> struct traits : public Testable {}; + /* ************************************************************************* */ template VariableSlots::VariableSlots(const FG& factorGraph) diff --git a/gtsam/inference/tests/testKey.cpp b/gtsam/inference/tests/testKey.cpp index 5b57096cb8..1033c0cc90 100644 --- a/gtsam/inference/tests/testKey.cpp +++ b/gtsam/inference/tests/testKey.cpp @@ -14,14 +14,14 @@ * @author Alex Cunningham */ -#include // for operator += -using namespace boost::assign; - -#include +#include #include #include -#include +#include + +#include // for operator += +using namespace boost::assign; using namespace std; using namespace gtsam; @@ -65,6 +65,13 @@ TEST(Key, KeySymbolEncoding) { EXPECT(assert_equal(symbol, Symbol(key))); } +/* ************************************************************************* */ +TEST(Key, ChrTest) { + Key key = Symbol('c',3); + EXPECT(Symbol::ChrTest('c')(key)); + EXPECT(!Symbol::ChrTest('d')(key)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/inference/tests/testLabeledSymbol.cpp b/gtsam/inference/tests/testLabeledSymbol.cpp index 07727c8dcc..18216453de 100644 --- a/gtsam/inference/tests/testLabeledSymbol.cpp +++ b/gtsam/inference/tests/testLabeledSymbol.cpp @@ -14,20 +14,19 @@ * @author Alex Cunningham */ -#include // for operator += -using namespace boost::assign; - -#include +#include #include #include -#include - +#include +#include // for operator += + +using namespace boost::assign; using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) { +TEST(LabeledSymbol, KeyLabeledSymbolConversion ) { LabeledSymbol expected('x', 'A', 4); Key key(expected); LabeledSymbol actual(key); @@ -36,7 +35,7 @@ TEST( testLabeledSymbol, KeyLabeledSymbolConversion ) { } /* ************************************************************************* */ -TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) { +TEST(LabeledSymbol, KeyLabeledSymbolEncoding ) { // Test encoding of LabeledSymbol <-> size_t <-> string // Encoding scheme: @@ -69,6 +68,17 @@ TEST( testLabeledSymbol, KeyLabeledSymbolEncoding ) { } } +/* ************************************************************************* */ +TEST(LabeledSymbol, ChrTest) { + Key key = LabeledSymbol('c','A',3); + EXPECT(LabeledSymbol::TypeTest('c')(key)); + EXPECT(!LabeledSymbol::TypeTest('d')(key)); + EXPECT(LabeledSymbol::LabelTest('A')(key)); + EXPECT(!LabeledSymbol::LabelTest('D')(key)); + EXPECT(LabeledSymbol::TypeLabelTest('c','A')(key)); + EXPECT(!LabeledSymbol::TypeLabelTest('c','D')(key)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 0505f6c018..5e7e08f618 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 6e85093095..20e0c52624 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 @@ class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationPar /*********************************************************************************************/ /* * 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 13da360a5e..44d68be838 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 69a70a5e4f..a89e7e21ca 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 5185154b41..1b2ad47e03 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 9cce29d60e..e0a8208195 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 8c8e2cb2b0..c676363411 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -20,8 +20,9 @@ #pragma once -#include #include +#include +#include namespace gtsam { @@ -99,7 +100,7 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const = 0; - /// Return the diagonal of the Hessian for this factor (raw memory version) + /// Raw memory access version of hessianDiagonal virtual void hessianDiagonal(double* d) const = 0; /// Return the block diagonal of the Hessian for this factor @@ -121,18 +122,15 @@ namespace gtsam { /// y += alpha * A'*A*x virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0; - /// y += alpha * A'*A*x - virtual void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const = 0; - - /// y += alpha * A'*A*x - virtual void multiplyHessianAdd(double alpha, const double* x, double* y) const = 0; - /// A'*b for Jacobian, eta for Hessian virtual VectorValues gradientAtZero() const = 0; - /// A'*b for Jacobian, eta for Hessian (raw memory version) + /// Raw memory access version of gradientAtZero 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 +141,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 54e721cd7b..6953d29693 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); } @@ -355,15 +357,6 @@ namespace gtsam { f->multiplyHessianAdd(alpha, x, y); } - /* ************************************************************************* */ - void GaussianFactorGraph::multiplyHessianAdd(double alpha, - const double* x, double* y) const { - vector FactorKeys = getkeydim(); - BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) - f->multiplyHessianAdd(alpha, x, y, FactorKeys); - - } - /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { multiplyInPlace(x, e.begin()); @@ -393,15 +386,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 910b25d1e7..811c0f8b04 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -310,10 +310,6 @@ namespace gtsam { void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - ///** y += alpha*A'A*x */ - void multiplyHessianAdd(double alpha, const double* x, - double* y) const; - ///** In-place version e <- A*x that overwrites e. */ void multiplyInPlace(const VectorValues& x, Errors& e) const; @@ -344,4 +340,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 f282682b31..453e598923 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -359,20 +359,10 @@ VectorValues HessianFactor::hessianDiagonal() const { } /* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +// Raw memory access version should be called in Regular Factors only currently void HessianFactor::hessianDiagonal(double* d) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - const Matrix& B = info_(pos, pos).selfadjointView(); - DMap(d + 9 * j) += B.diagonal(); - } + throw std::runtime_error( + "HessianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only"); } /* ************************************************************************* */ @@ -548,48 +538,6 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, } } -/* ************************************************************************* */ -void HessianFactor::multiplyHessianAdd(double alpha, const double* x, - double* yvalues, vector offsets) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - // Create a vector of temporary y values, corresponding to rows i - vector y; - y.reserve(size()); - for (const_iterator it = begin(); it != end(); it++) - y.push_back(zero(getDim(it))); - - // Accessing the VectorValues one by one is expensive - // So we will loop over columns to access x only once per column - // And fill the above temporary y values, to be added into yvalues after - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - DenseIndex i = 0; - for (; i < j; ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // blocks on the diagonal are only half - y[i] += info_(j, j).selfadjointView() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - // for below diagonal, we take transpose block from upper triangular part - for (i = j + 1; i < (DenseIndex) size(); ++i) - y[i] += info_(i, j).knownOffDiagonal() - * ConstDMap(x + offsets[keys_[j]], - offsets[keys_[j] + 1] - offsets[keys_[j]]); - } - - // copy to yvalues - for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) - DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += - alpha * y[i]; -} - - /* ************************************************************************* */ VectorValues HessianFactor::gradientAtZero() const { VectorValues g; @@ -600,20 +548,34 @@ VectorValues HessianFactor::gradientAtZero() const { } /* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +// Raw memory access version should be called in Regular Factors only currently void HessianFactor::gradientAtZero(double* d) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { - Key j = keys_[pos]; - // Get the diagonal block, and insert its diagonal - DVector dj = -info_(pos,size()).knownOffDiagonal(); - DMap(d + 9 * j) += dj; + throw std::runtime_error( + "HessianFactor::gradientAtZero raw memory access is allowed for Regular Factors only"); +} + +/* ************************************************************************* */ +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; } /* ************************************************************************* */ diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 62d84925c3..553978e375 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -340,7 +340,7 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; - /* ************************************************************************* */ + /// Raw memory access version of hessianDiagonal virtual void hessianDiagonal(double* d) const; /// Return the block diagonal of the Hessian for this factor @@ -380,15 +380,18 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; - - void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; - /// eta for Hessian VectorValues gradientAtZero() const; + /// Raw memory access version of gradientAtZero 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 +440,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 a63bbf4730..eba06f99ae 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -461,22 +461,9 @@ VectorValues JacobianFactor::hessianDiagonal() const { } /* ************************************************************************* */ -// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n +// Raw memory access version should be called in Regular Factors only currently void JacobianFactor::hessianDiagonal(double* d) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - - // Loop over all variables in the factor - for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { - // Get the diagonal block, and insert its diagonal - DVector dj; - for (size_t k = 0; k < 9; ++k) - dj(k) = Ab_(j).col(k).squaredNorm(); - - DMap(d + 9 * j) += dj; - } + throw std::runtime_error("JacobianFactor::hessianDiagonal raw memory access is allowed for Regular Factors only"); } /* ************************************************************************* */ @@ -528,40 +515,6 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, transposeMultiplyAdd(alpha, Ax, y); } -void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, - double* y, std::vector keys) const { - - // Use eigen magic to access raw memory - typedef Eigen::Matrix DVector; - typedef Eigen::Map DMap; - typedef Eigen::Map ConstDMap; - - if (empty()) - return; - Vector Ax = zero(Ab_.rows()); - - // Just iterate over all A matrices and multiply in correct config part - for (size_t pos = 0; pos < size(); ++pos) - Ax += Ab_(pos) - * ConstDMap(x + keys[keys_[pos]], - keys[keys_[pos] + 1] - keys[keys_[pos]]); - - // Deal with noise properly, need to Double* whiten as we are dividing by variance - if (model_) { - model_->whitenInPlace(Ax); - model_->whitenInPlace(Ax); - } - - // multiply with alpha - Ax *= alpha; - - // Again iterate over all A matrices and insert Ai^e into y - for (size_t pos = 0; pos < size(); ++pos) - DMap(y + keys[keys_[pos]], keys[keys_[pos] + 1] - keys[keys_[pos]]) += Ab_( - pos).transpose() * Ax; - -} - /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; @@ -574,8 +527,16 @@ VectorValues JacobianFactor::gradientAtZero() const { } /* ************************************************************************* */ +// Raw memory access version should be called in Regular Factors only currently void JacobianFactor::gradientAtZero(double* d) const { - //throw std::runtime_error("gradientAtZero not implemented for Jacobian factor"); + throw std::runtime_error("JacobianFactor::gradientAtZero raw memory access is allowed for Regular Factors only"); +} + +/* ************************************************************************* */ +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 92f43b6be4..02ae21566f 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? @@ -281,16 +283,15 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - void multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) const; - - void multiplyHessianAdd(double alpha, const double* x, double* y) const {}; - /// 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 +356,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 dc6801e032..7aa210dadc 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 27eb57b44a..ffb744239d 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 */ - - /* reset y */ - Ax.setZero(); - - 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 */ - - /* 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))); - } - - 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; - } - } - - /* 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::multiply(const Vector &x, Vector& AtAx) const { + /* implement A^T*(A*x), assume x and AtAx are pre-allocated */ + + // Build a VectorValues for Vector x + VectorValues vvX = buildVectorValues(x,keyInfo_); + + // VectorValues form of A'Ax for multiplyHessianAdd + VectorValues vvAtAx; + + // vvAtAx += 1.0 * A'Ax for each factor + gfg_.multiplyHessianAdd(1.0, vvX, vvAtAx); + + // Make the result as Vector form + AtAx = vvAtAx.vector(); + } /*****************************************************************************/ void GaussianFactorGraphSystem::getb(Vector &b) const { /* compute rhs, assume b pre-allocated */ - /* reset */ - b.setZero(); - - 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."); - } - } + // Get whitened r.h.s (A^T * b) from each factor in the form of VectorValues + VectorValues vvb = gfg_.gradientAtZero(); + + // 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 9af362fba3..538d886b47 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; @@ -125,7 +126,9 @@ void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const void BlockJacobiPreconditioner::build( const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) { + // n is the number of keys const size_t n = keyInfo.size(); + // dims_ is a vector that contains the dimension of keys dims_ = keyInfo.colSpec(); /* prepare the buffer of block diagonals */ @@ -141,11 +144,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 +160,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 10ceb78a91..623b298636 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 @@ class GTSAM_EXPORT Preconditioner { /* 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 290249b68a..ce33116ab4 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/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index d87d842de1..7404caa14c 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -315,30 +315,6 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) EXPECT(assert_equal(2*expected, actual)); } -/* ************************************************************************* */ -TEST( GaussianFactorGraph, multiplyHessianAdd3 ) -{ - GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); - - // brute force - Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); - Vector X(6); X<<1,2,3,4,5,6; - Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450; - EXPECT(assert_equal(Y,AtA*X)); - - double* x = &X[0]; - - Vector fast_y = gtsam::zero(6); - gfg.multiplyHessianAdd(1.0, x, fast_y.data()); - EXPECT(assert_equal(Y, fast_y)); - - // now, do it with non-zero y - gfg.multiplyHessianAdd(1.0, x, fast_y.data()); - EXPECT(assert_equal(2*Y, fast_y)); - -} - - /* ************************************************************************* */ TEST( GaussianFactorGraph, matricesMixed ) { @@ -351,7 +327,6 @@ TEST( GaussianFactorGraph, matricesMixed ) EXPECT(assert_equal(A.transpose()*b, eta)); } - /* ************************************************************************* */ TEST( GaussianFactorGraph, gradientAtZero ) { diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index cba65580e8..3a2cd0fd40 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 137f102b3a..2f03d72a41 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 e62a24ccaa..fd4b9be874 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 @@ class AHRSFactor: public NoiseModelFactor3 { * 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 @@ class AHRSFactor: public NoiseModelFactor3 { PreintegratedMeasurements(const Vector3& bias, const Matrix3& measuredOmegaCovariance); - /// 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_; } + /// print + void print(const std::string& s = "Preintegrated Measurements: ") const; + + /// equals + bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const; + /// TODO: Document void resetIntegration(); @@ -103,12 +87,6 @@ class AHRSFactor: public NoiseModelFactor3 { 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 @@ class AHRSFactor: public NoiseModelFactor3 { 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 @@ class AHRSFactor: public NoiseModelFactor3 { 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 @@ class AHRSFactor: public NoiseModelFactor3 { /// Access the preintegrated measurements. const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; + return _PIM_; } const Vector3& omegaCoriolis() const { @@ -208,7 +183,7 @@ class AHRSFactor: public NoiseModelFactor3 { 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 7eba995d3e..089747dc6e 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 def36cc675..08b75c00d6 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -54,7 +54,7 @@ class AttitudeFactor { /** 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 @@ class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactor1, public At } }; +/// traits +template<> struct traits : public Testable {}; + /** * Version of AttitudeFactor for Pose3 * @addtogroup Navigation @@ -212,5 +215,8 @@ class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactor1, } }; +/// traits +template<> struct traits : public Testable {}; + } /// namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index a2ce631eab..1c22bab9a2 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); - - // 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 - } + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - 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(); - - // 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, according to [3] - /* ---------------------------------------------------------------------------------------------------- */ - 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(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; + // 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(); + + Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i; + Matrix93 D_r_vel_i, D_r_vel_j; + + // error wrt preintegrated measurements + Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, + bias_i, gravity_, omegaCoriolis_, use2ndOrderCoriolis_, + H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, + H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); + + // if we need the jacobians + if (H1) { + H1->resize(15, 6); + H1->block < 9, 6 > (0, 0) = D_r_pose_i; + // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] + H1->block < 6, 6 > (9, 0) = Z_6x6; } - - if(H2) { - H2->resize(15,3); - (*H2) << - // 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 (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) << - // 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 (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) << - // dfP/dVj - Z_3x3, - // dfV/dVj - I_3x3, - // dfR/dVj - Z_3x3, - //dBiasAcc/dVj - Z_3x3, - //dBiasOmega/dVj - Z_3x3; + 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) { - 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 (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) << - // 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; + 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; } - // 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 5e2bfeadbe..a7c6ecd396 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 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 + + 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 + + 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 @@ class CombinedImuFactor: public NoiseModelFactor6 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 @@ class CombinedImuFactor: public 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 @@ class CombinedImuFactor: public NoiseModelFactor6 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 @@ class CombinedImuFactor: public NoiseModelFactor6(*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 fd769b188d..bfd3ebb521 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 466d932033..ce93bd7405 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 - /// @{ - - /** 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(); } + /// @name Deprecated + /// @{ + 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 9a9cc9d62c..0aaa0122e6 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) { - - // NOTE: order is important here because each update uses old values (i.e., we have to update - // jacobians and covariances before updating preintegrated measurements). - - // 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 - } + boost::optional body_P_sensor, + OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) { - 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 + Vector3 correctedAcc, correctedOmega; + correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); - 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 (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); - // Update preintegrated measurements covariance + // 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 ; - - // 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; + // 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; + + // 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 -{ - - 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); +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 { + + 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 2af634926b..b91c76ade7 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 @@ class ImuFactor: public NoiseModelFactor5 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; - } + Matrix preintMeasCov() const { return preintMeasCov_;} + + private: - // 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); + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); } - /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */ - - 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_); - } - }; + }; 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 + typedef ImuFactor This; + typedef NoiseModelFactor5 Base; - bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + PreintegratedMeasurements _PIM_; public: - /** Shorthand for a smart pointer to a factor */ + /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 - typedef typename boost::shared_ptr shared_ptr; + typedef typename boost::shared_ptr shared_ptr; #else - typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; #endif - /** Default constructor - only use for serialization */ - ImuFactor(); + /** 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 */ + /** + * 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); - /// print - virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual ~ImuFactor() {} - /// equals - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const; - /** Access the preintegrated measurements. */ + /** implement functions needed for Testable */ - const PreintegratedMeasurements& preintegratedMeasurements() const { - return preintegratedMeasurements_; } + /// print + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - const Vector3& gravity() const { return gravity_; } + /// equals + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const; - const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + /** Access the preintegrated measurements. */ - /** implement functions needed to derive from Factor */ + const PreintegratedMeasurements& preintegratedMeasurements() const { + return _PIM_; } - /// 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; + /** implement functions needed to derive from Factor */ - /// 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); + /// 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(preintegratedMeasurements_); - ar & BOOST_SERIALIZATION_NVP(gravity_); - ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); - } - }; // class ImuFactor + /** 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; +typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements; } /// namespace gtsam diff --git a/gtsam/navigation/ImuFactorBase.h b/gtsam/navigation/ImuFactorBase.h new file mode 100644 index 0000000000..1b7919a822 --- /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 0000000000..67deb2d998 --- /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 0000000000..8214c1930d --- /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 e6ecf42a38..25b3518bc9 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 aab38f2585..d14eafb7d2 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) { + + 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){ -ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( + 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); - - 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); + 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 ); - // 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); - + Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); - Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); + // Create factor + ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); - Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2); + 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); - 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 cf81c32aee..0c4c9e3a6d 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(bias.head(3), bias.tail(3)); - // 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); - // Create factor - ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); + // 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); - 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 - Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0; - // EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); + // 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); - 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 694163ede0..950c6ce635 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 d5cd3e809f..7295f31604 100644 --- a/gtsam/nonlinear/AdaptAutoDiff.h +++ b/gtsam/nonlinear/AdaptAutoDiff.h @@ -18,18 +18,60 @@ #pragma once +#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; @@ -38,29 +80,22 @@ 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: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; - - T operator()(const A1& a1, const A2& a2, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) { + T operator()(const A1& a1, const A2& a2, OptionalJacobian H1 = boost::none, + OptionalJacobian H2 = boost::none) { 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; @@ -86,7 +121,7 @@ class AdaptAutoDiff { 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 332c23ca78..a86e7312a9 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,10 +19,9 @@ #pragma once -#include +#include #include -#include -#include +#include #include #include @@ -33,6 +32,7 @@ #include namespace MPL = boost::mpl::placeholders; +#include // operator typeid #include class ExpressionFactorBinaryTest; @@ -42,21 +42,21 @@ namespace gtsam { const unsigned TraceAlignment = 16; -template -T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment){ +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { // right now only word sized types are supported. // Easy to extend if needed, // by somehow inferring the unsigned integer of same size BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); size_t & uiValue = reinterpret_cast(value); size_t misAlignment = uiValue % requiredAlignment; - if(misAlignment) { + if (misAlignment) { uiValue += requiredAlignment - misAlignment; } return value; } -template -T upAligned(T value, unsigned requiredAlignment = TraceAlignment){ +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { return upAlign(value, requiredAlignment); } @@ -88,19 +88,21 @@ class JacobianMap { namespace internal { -template +template struct UseBlockIf { static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key){ + JacobianMap& jacobians, Key key) { // block makes HUGE difference - jacobians(key).block(0, 0) += dTdA; - }; + jacobians(key).block( + 0, 0) += dTdA; + } + ; }; /// Handle Leaf Case for Dynamic Matrix type (slower) -template +template struct UseBlockIf { static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { + JacobianMap& jacobians, Key key) { jacobians(key) += dTdA; } }; @@ -111,10 +113,9 @@ template void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { internal::UseBlockIf< - Derived::RowsAtCompileTime != Eigen::Dynamic && - Derived::ColsAtCompileTime != Eigen::Dynamic, - Derived> - ::addToJacobian(dTdA, jacobians, key); + Derived::RowsAtCompileTime != Eigen::Dynamic + && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( + dTdA, jacobians, key); } //----------------------------------------------------------------------------- @@ -141,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; @@ -245,6 +246,15 @@ class ExpressionNode { virtual ~ExpressionNode() { } + /// Streaming + GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, + const ExpressionNode& node) { + os << "Expression of type " << typeid(T).name(); + if (node.traceSize_>0) os << ", trace size = " << node.traceSize_; + os << "\n"; + return os; + } + /// Return keys that play in this expression as a set virtual std::set keys() const { std::set keys; @@ -265,7 +275,7 @@ class ExpressionNode { /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const = 0; + ExecutionTraceStorage* traceStorage) const = 0; }; //----------------------------------------------------------------------------- @@ -297,57 +307,11 @@ class ConstantExpression: public ExpressionNode { } }; -//----------------------------------------------------------------------------- -/// 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 @@ -372,7 +336,7 @@ class LeafExpression > : public ExpressionNode { /// Return dimensions for each argument virtual void dims(std::map& map) const { - map[key_] = traits::dimension::value; + map[key_] = traits::dimension; } /// Return value @@ -448,16 +412,15 @@ class LeafExpression > : public ExpressionNode { /// 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 OptionalJacobian { - typedef Eigen::Matrix::value, - traits::dimension::value> Jacobian; - typedef boost::optional type; +struct MakeOptionalJacobian { + typedef OptionalJacobian::dimension, + traits::dimension> type; }; /** @@ -560,8 +523,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 Eigen::Matrix & dFdT, + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { Base::Record::reverseAD4(dFdT, jacobians); This::trace.reverseAD1(dFdT * This::dTdA, jacobians); @@ -569,7 +532,7 @@ struct GenerateFunctionalNode: Argument, Base { }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, + void trace(const Values& values, Record* record, ExecutionTraceStorage*& traceStorage) const { Base::trace(values, record, traceStorage); // recurse // Write an Expression execution trace in record->trace @@ -623,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; @@ -645,7 +608,7 @@ struct FunctionalNode { }; /// Construct an execution trace for reverse AD - Record* trace(const Values& values, + Record* trace(const Values& values, ExecutionTraceStorage* traceStorage) const { assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); @@ -668,9 +631,11 @@ struct FunctionalNode { template class UnaryExpression: public FunctionalNode >::type { + typedef typename MakeOptionalJacobian::type OJ1; + public: - typedef boost::function::type)> Function; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -710,13 +675,15 @@ class UnaryExpression: public FunctionalNode >::type { /// Binary Expression template -class BinaryExpression: public FunctionalNode >::type { +class BinaryExpression: + public FunctionalNode >::type { + + typedef typename MakeOptionalJacobian::type OJ1; + typedef typename MakeOptionalJacobian::type OJ2; public: - typedef boost::function< - T(const A1&, const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -764,14 +731,16 @@ class BinaryExpression: public FunctionalNode >::t /// Ternary Expression template -class TernaryExpression: public FunctionalNode >::type { +class TernaryExpression: + public FunctionalNode >::type { + + typedef typename MakeOptionalJacobian::type OJ1; + typedef typename MakeOptionalJacobian::type OJ2; + typedef typename MakeOptionalJacobian::type OJ3; public: - typedef boost::function< - T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, - typename OptionalJacobian::type, - typename OptionalJacobian::type)> Function; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -787,7 +756,8 @@ class TernaryExpression: public FunctionalNode this->template reset(e2.root()); this->template reset(e3.root()); ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + e3.traceSize(); + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + + e3.traceSize(); } friend class Expression ; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index c7f0227243..a39b1557c6 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -20,8 +20,8 @@ #pragma once #include -#include #include +#include #include #include @@ -52,6 +52,11 @@ class Expression { public: + /// Print + void print(const std::string& s) const { + std::cout << s << *root_ << std::endl; + } + // Construct a constant expression Expression(const T& value) : root_(new ConstantExpression(value)) { @@ -75,7 +80,7 @@ class Expression { /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename OptionalJacobian::type) const) : + T (A::*method)(typename MakeOptionalJacobian::type) const) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -89,8 +94,8 @@ class Expression { /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename OptionalJacobian::type, - typename OptionalJacobian::type) 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), @@ -104,6 +109,20 @@ class Expression { root_(new BinaryExpression(function, expression1, expression2)) { } + /// Construct a binary method expression + template + Expression(const Expression& expression1, + T (A1::*method)(const A2&, const A3&, + typename TernaryExpression::OJ1, + typename TernaryExpression::OJ2, + typename TernaryExpression::OJ3) const, + const Expression& expression2, const Expression& expression3) : + root_( + new TernaryExpression( + boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, + expression2, expression3)) { + } + /// Construct a ternary function expression template Expression(typename TernaryExpression::Function function, @@ -151,6 +170,15 @@ class Expression { return root_->value(values); } + /** + * @return a "deep" copy of this Expression + * "deep" is in quotes because the ExpressionNode hierarchy is *not* cloned. + * The intent is for derived classes to be copied using only a Base pointer. + */ + virtual boost::shared_ptr clone() const { + return boost::make_shared(*this); + } + private: /// Vaguely unsafe keys and dimensions in same order @@ -173,7 +201,7 @@ class Expression { 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); @@ -206,10 +234,24 @@ class Expression { // with an execution trace, made up entirely of "Record" structs, see // the FunctionalNode class in expression-inl.h size_t size = traceSize(); + + // Windows does not support variable length arrays, so memory must be dynamically + // allocated on Visual Studio. For more information see the issue below + // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio +#ifdef _MSC_VER + ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size]; +#else ExecutionTraceStorage traceStorage[size]; +#endif + ExecutionTrace trace; T value(traceExecution(values, trace, traceStorage)); trace.startReverseAD1(jacobians); + +#ifdef _MSC_VER + delete[] traceStorage; +#endif + return value; } @@ -223,10 +265,9 @@ class Expression { template struct apply_compose { typedef T result_type; - static const int Dim = traits::dimension::value; - typedef Eigen::Matrix Jacobian; - T operator()(const T& x, const T& y, boost::optional H1, - boost::optional H2) const { + 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 de6e4983a0..4769e50489 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -32,11 +32,13 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { +protected: + T measurement_; ///< the measurement to be compared with the expression 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: @@ -63,13 +65,12 @@ class ExpressionFactor: public NoiseModelFactor { */ 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); } } @@ -97,8 +98,7 @@ class ExpressionFactor: public NoiseModelFactor { 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); @@ -109,5 +109,5 @@ class ExpressionFactor: public NoiseModelFactor { }; // ExpressionFactor -} +} // \ namespace gtsam diff --git a/gtsam/nonlinear/ExpressionFactorGraph.h b/gtsam/nonlinear/ExpressionFactorGraph.h new file mode 100644 index 0000000000..122bd429f6 --- /dev/null +++ b/gtsam/nonlinear/ExpressionFactorGraph.h @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExpressionFactorGraph.h + * @brief Factor graph that supports adding ExpressionFactors directly + * @author Frank Dellaert + * @date December 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Factor graph that supports adding ExpressionFactors directly + */ +class ExpressionFactorGraph: public NonlinearFactorGraph { + +public: + + /// @name Adding Factors + /// @{ + + /** + * Directly add ExpressionFactor that implements |h(x)-z|^2_R + * @param h expression that implements measurement function + * @param z measurement + * @param R model + */ + template + void addExpressionFactor(const Expression& h, const T& z, + const SharedNoiseModel& R) { + push_back(boost::make_shared >(R, z, h)); + } + + /// @} +}; + +} diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 5765eca011..9251aac070 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 7bbd14980c..4adad676e0 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 36ebd70330..f7c6d03453 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 c2e728a2be..9adceee6a9 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -633,6 +633,9 @@ class GTSAM_EXPORT ISAM2: public BayesTree { }; // 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 340f3f6bc2..676b3fb856 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -155,5 +155,7 @@ class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor { }; // \class LinearContainerFactor +template<> struct traits : public Testable {}; + } // \namespace gtsam diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 10174181b6..6e348fb58b 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -23,6 +23,7 @@ #include #include +#include namespace gtsam { @@ -92,8 +93,9 @@ class NonlinearEquality: public NoiseModelFactor1 { */ 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 @@ class NonlinearEquality: public NoiseModelFactor1 { */ 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 @@ class NonlinearEquality: public NoiseModelFactor1 { 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 @@ class NonlinearEquality: public NoiseModelFactor1 { /** 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 @@ class NonlinearEquality: public NoiseModelFactor1 { }; // \class NonlinearEquality +template +struct traits > : Testable > { +}; + /* ************************************************************************* */ /** * Simple unary equality constraint - fixes a value for a variable @@ -231,8 +237,8 @@ class NonlinearEquality1: public NoiseModelFactor1 { * */ 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 @@ class NonlinearEquality1: public NoiseModelFactor1 { 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 @@ class NonlinearEquality1: public NoiseModelFactor1 { }; // \NonlinearEquality1 +template +struct traits > : Testable > { +}; + /* ************************************************************************* */ /** * Simple binary equality constraint - this constraint forces two factors to @@ -302,7 +312,7 @@ class NonlinearEquality2: public NoiseModelFactor2 { ///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 @@ class NonlinearEquality2: public NoiseModelFactor2 { /** 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 @@ class NonlinearEquality2: public NoiseModelFactor2 { }; // \NonlinearEquality2 +template +struct traits > : Testable > { +}; + + }// namespace gtsam diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 9fd4f83832..5d9f176b35 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -20,14 +20,14 @@ #pragma once -#include -#include - #include #include #include #include +#include +#include +#include /** * Macro to add a standard clone function to a derived factor @@ -144,6 +144,10 @@ class GTSAM_EXPORT NonlinearFactor: public Factor { }; // \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 35e532262f..28f72d57d0 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 e983ccb175..fe2c3f3cac 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -289,40 +289,16 @@ namespace gtsam { } /* ************************************************************************* */ - // insert a plain value using the default chart + // insert a templated value 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 + // update with templated value 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.cpp b/gtsam/nonlinear/Values.cpp index b92e541439..372bced8ea 100644 --- a/gtsam/nonlinear/Values.cpp +++ b/gtsam/nonlinear/Values.cpp @@ -112,6 +112,24 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + Vector Values::atFixed(Key j, size_t n) { + switch (n) { + case 1: return at(j); + case 2: return at(j); + case 3: return at(j); + case 4: return at(j); + case 5: return at(j); + case 6: return at(j); + case 7: return at(j); + case 8: return at(j); + case 9: return at(j); + default: + throw runtime_error( + "Values::at fixed size can only handle n in 1..9"); + } + } + /* ************************************************************************* */ const Value& Values::at(Key j) const { // Find the item @@ -130,6 +148,24 @@ namespace gtsam { throw ValuesKeyAlreadyExists(j); } + /* ************************************************************************* */ + void Values::insertFixed(Key j, const Vector& v, size_t n) { + switch (n) { + case 1: insert(j,v); break; + case 2: insert(j,v); break; + case 3: insert(j,v); break; + case 4: insert(j,v); break; + case 5: insert(j,v); break; + case 6: insert(j,v); break; + case 7: insert(j,v); break; + case 8: insert(j,v); break; + case 9: insert(j,v); break; + default: + throw runtime_error( + "Values::insert fixed size can only handle n in 1..9"); + } + } + /* ************************************************************************* */ void Values::insert(const Values& values) { for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index d00baa0d95..73d0711be5 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 @@ -180,6 +180,13 @@ namespace gtsam { template const ValueType& at(Key j) const; + /// Special version for small fixed size vectors, for matlab/python + /// throws truntime error if n<1 || n>9 + Vector atFixed(Key j, size_t n); + + /// version for double + double atDouble(size_t key) const { return at(key);} + /** Retrieve a variable by key \c j. This version returns a reference * to the base Value class, and needs to be casted before use. * @param j Retrieve the value associated with this key @@ -253,19 +260,16 @@ namespace gtsam { /** Templated version to add a variable with the given j, * throws KeyAlreadyExists if j is already present - * if no chart is specified, the DefaultChart is used */ template void insert(Key j, const ValueType& val); - /// overloaded insert version that also specifies a chart - template - void insert(Key j, const ValueType& val); - - /// overloaded insert version that also specifies a chart initializer - template - void insert(Key j, const ValueType& val, Chart chart); + /// Special version for small fixed size vectors, for matlab/python + /// throws truntime error if n<1 || n>9 + void insertFixed(Key j, const Vector& v, size_t n); + /// version for double + void insertDouble(Key j, double c) { insert(j,c); } /** insert that mimics the STL map insert - if the value already exists, the map is not modified * and an iterator to the existing value is returned, along with 'false'. If the value did not @@ -283,14 +287,6 @@ namespace gtsam { template void update(Key j, const T& val); - /// overloaded insert version that also specifies a chart - template - void update(Key j, const T& val); - - /// overloaded insert version that also specifies a chart initializer - template - void update(Key j, const T& val, Chart chart); - /** update the current available values without adding new ones */ void update(const Values& values); @@ -509,6 +505,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 4fd47eb760..ab6703f3a8 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 79% rename from gtsam_unstable/nonlinear/expressions.h rename to gtsam/nonlinear/expressions.h index 2490100d6f..a549517e57 100644 --- a/gtsam_unstable/nonlinear/expressions.h +++ b/gtsam/nonlinear/expressions.h @@ -8,15 +8,14 @@ #pragma once #include -#include +#include namespace gtsam { // Generics - -template +template Expression between(const Expression& t1, const Expression& t2) { - return Expression(t1, &T::between, t2); + return Expression(traits::Between, t1, t2); } typedef Expression double_; diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 7d31616c5d..f3858c8187 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 1cc6749012..483b5ffa95 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/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index dfa60e54ea..80100af5e4 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include @@ -34,8 +33,8 @@ using namespace gtsam; /* ************************************************************************* */ template -Point2 uncalibrate(const CAL& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { +Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal, + OptionalJacobian<2, 2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -75,13 +74,10 @@ TEST(Expression, Leaves) { /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f0(const Point3& p, boost::optional H) { +Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { return Point2(); } -LieScalar f1(const Point3& p, boost::optional&> H) { - return LieScalar(0.0); -} -double f2(const Point3& p, boost::optional&> H) { +double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } Expression p(1); @@ -92,11 +88,6 @@ TEST(Expression, Unary0) { Expression e(f0, p); EXPECT(expected == e.keys()); } -TEST(Expression, Unary1) { - using namespace unary; - Expression e(f1, p); - EXPECT(expected == e.keys()); -} TEST(Expression, Unary2) { using namespace unary; Expression e(f2, p); @@ -117,7 +108,7 @@ TEST(Expression, NullaryMethod) { // Check dims as map std::map map; norm.dims(map); - LONGS_EQUAL(1,map.size()); + LONGS_EQUAL(1, map.size()); // Get value and Jacobians std::vector H(1); @@ -133,9 +124,8 @@ TEST(Expression, NullaryMethod) { // Binary(Leaf,Leaf) namespace binary { // Create leaves -double doubleF(const Pose3& pose, const Point3& point, - boost::optional&> H1, - boost::optional&> H2) { +double doubleF(const Pose3& pose, // + const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) { return 0.0; } Expression x(1); @@ -244,8 +234,7 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp similarity index 97% rename from gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp rename to gtsam/nonlinear/tests/testExpressionFactor.cpp index 09cad558a4..99b8120e30 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 @@ -131,14 +131,14 @@ TEST(ExpressionFactor, Unary) { // Unary(Leaf)) and Unary(Unary(Leaf))) // wide version (not handled in fixed-size pipeline) typedef Eigen::Matrix Matrix93; -Vector9 wide(const Point3& p, boost::optional H) { +Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) { Vector9 v; v << p.vector(), p.vector(), p.vector(); if (H) *H << eye(3), eye(3), eye(3); return v; } typedef Eigen::Matrix Matrix9; -Vector9 id9(const Vector9& v, boost::optional H) { +Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) { if (H) *H = Matrix9::Identity(); return v; } @@ -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); @@ -161,7 +162,7 @@ TEST(ExpressionFactor, Wide) { /* ************************************************************************* */ static Point2 myUncal(const Cal3_S2& K, const Point2& p, - boost::optional Dcal, boost::optional Dp) { + OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) { return K.uncalibrate(p, Dcal, Dp); } @@ -427,8 +428,7 @@ TEST(ExpressionFactor, compose3) { /* ************************************************************************* */ // Test compose with three arguments Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - boost::optional H1, boost::optional H2, - boost::optional H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index a8f287d1ea..2bdc04d5bc 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 941728d8cc..faa285cd5d 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 9c5df7ea09..319c74cd5b 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 9142f9d3d8..2f8dd601f7 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 7d31e2e2cd..f7d42497ef 100644 --- a/gtsam/slam/PoseRotationPrior.h +++ b/gtsam/slam/PoseRotationPrior.h @@ -29,6 +29,10 @@ class PoseRotationPrior : public NoiseModelFactor1 { 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 @@ class PoseRotationPrior : public NoiseModelFactor1 { /** 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 2d9b3fdd4d..6689653aa5 100644 --- a/gtsam/slam/PoseTranslationPrior.h +++ b/gtsam/slam/PoseTranslationPrior.h @@ -62,7 +62,8 @@ class PoseTranslationPrior : public NoiseModelFactor1 { 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 8fbbd4d885..eee14f9f2e 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 db37e6b8de..52e28beaf0 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 af1c1a1bd8..216b9320e1 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -16,99 +16,182 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { - /** - * Binary factor for a range measurement - * @addtogroup SLAM - */ - template - class RangeFactor: public NoiseModelFactor2 { - private: - - double measured_; /** measurement */ - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame - - typedef RangeFactor This; - typedef NoiseModelFactor2 Base; - - typedef POSE Pose; - typedef POINT Point; - - // Concept requirements for this factor - GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, POINT) - - public: - - RangeFactor() {} /* Default constructor */ - - RangeFactor(Key poseKey, Key pointKey, double measured, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : - Base(model, poseKey, pointKey), measured_(measured), body_P_sensor_(body_P_sensor) { - } - - virtual ~RangeFactor() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** h(x)-z */ - Vector evaluateError(const POSE& pose, const POINT& point, - boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - double hx; - if(body_P_sensor_) { - if(H1) { - Matrix H0; - hx = pose.compose(*body_P_sensor_, H0).range(point, H1, H2); - *H1 = *H1 * H0; - } else { - hx = pose.compose(*body_P_sensor_).range(point, H1, H2); - } - } else { - hx = pose.range(point, H1, H2); - } - return (Vector(1) << hx - measured_).finished(); - } - - /** return the measured */ - double measured() const { - return measured_; - } - - /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const This *e = dynamic_cast (&expected); - return e != NULL - && Base::equals(*e, tol) - && fabs(this->measured_ - e->measured_) < tol - && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); - } - - /** print contents */ - void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "RangeFactor, range = " << measured_ << std::endl; - if(this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); - Base::print("", keyFormatter); - } - - private: +/** + * Binary factor for a range measurement + * @addtogroup SLAM + */ +template +class RangeFactor: public NoiseModelFactor2 { +private: + + double measured_; /** measurement */ + + typedef RangeFactor This; + typedef NoiseModelFactor2 Base; + + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2) + +public: + + RangeFactor() { + } /* Default constructor */ + + RangeFactor(Key key1, Key key2, double measured, + const SharedNoiseModel& model) : + Base(model, key1, key2), measured_(measured) { + } + + virtual ~RangeFactor() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** h(x)-z */ + Vector evaluateError(const T1& t1, const T2& t2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + double hx; + hx = t1.range(t2, H1, H2); + return (Vector(1) << hx - measured_).finished(); + } + + /** return the measured */ + double measured() const { + return measured_; + } + + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && fabs(this->measured_ - e->measured_) < tol; + } + + /** print contents */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "RangeFactor, range = " << measured_ << std::endl; + Base::print("", keyFormatter); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + } +}; // \ RangeFactor + +/// traits +template +struct traits > : public Testable > {}; - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); +/** + * Binary factor for a range measurement, with a transform applied + * @addtogroup SLAM + */ +template +class RangeFactorWithTransform: public NoiseModelFactor2 { +private: + + double measured_; /** measurement */ + POSE body_P_sensor_; ///< The pose of the sensor in the body frame + + typedef RangeFactorWithTransform This; + typedef NoiseModelFactor2 Base; + + // Concept requirements for this factor + GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2) + +public: + + RangeFactorWithTransform() { + } /* Default constructor */ + + RangeFactorWithTransform(Key key1, Key key2, double measured, + const SharedNoiseModel& model, const POSE& body_P_sensor) : + Base(model, key1, key2), measured_(measured), body_P_sensor_( + body_P_sensor) { + } + + virtual ~RangeFactorWithTransform() { + } + + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + /** h(x)-z */ + Vector evaluateError(const POSE& t1, const T2& t2, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + double hx; + if (H1) { + Matrix H0; + hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2); + *H1 = *H1 * H0; + } else { + hx = t1.compose(body_P_sensor_).range(t2, H1, H2); } - }; // RangeFactor - -} // namespace gtsam + return (Vector(1) << hx - measured_).finished(); + } + + /** return the measured */ + double measured() const { + return measured_; + } + + /** equals specialized to this factor */ + virtual bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const { + const This *e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) + && fabs(this->measured_ - e->measured_) < tol + && body_P_sensor_.equals(e->body_P_sensor_); + } + + /** print contents */ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + std::cout << s << "RangeFactor, range = " << measured_ << std::endl; + this->body_P_sensor_.print(" sensor pose in body frame: "); + Base::print("", keyFormatter); + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +}; // \ RangeFactorWithTransform + +/// traits +template +struct traits > : public Testable > {}; + +} // \ namespace gtsam diff --git a/gtsam/slam/ReferenceFrameFactor.h b/gtsam/slam/ReferenceFrameFactor.h index 13dec7de31..6a70d58b43 100644 --- a/gtsam/slam/ReferenceFrameFactor.h +++ b/gtsam/slam/ReferenceFrameFactor.h @@ -84,7 +84,7 @@ class ReferenceFrameFactor : public NoiseModelFactor3 { * 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 @@ class ReferenceFrameFactor : public NoiseModelFactor3 { 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 @@ class ReferenceFrameFactor : public NoiseModelFactor3 { } }; +/// traits +template +struct traits > : public Testable > {}; + } // \namespace gtsam diff --git a/gtsam/slam/RegularHessianFactor.h b/gtsam/slam/RegularHessianFactor.h index 0e20bb7090..f3f90dc2d0 100644 --- a/gtsam/slam/RegularHessianFactor.h +++ b/gtsam/slam/RegularHessianFactor.h @@ -51,18 +51,12 @@ class RegularHessianFactor: public HessianFactor { HessianFactor(keys, augmentedInformation) { } - /** y += alpha * A'*A*x */ - void multiplyHessianAdd(double alpha, const VectorValues& x, - VectorValues& y) const { - HessianFactor::multiplyHessianAdd(alpha, x, y); - } - // Scratch space for multiplyHessianAdd typedef Eigen::Matrix DVector; mutable std::vector y; - void multiplyHessianAdd(double alpha, const double* x, - double* yvalues) const { + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const { // Create a vector of temporary y values, corresponding to rows i y.resize(size()); BOOST_FOREACH(DVector & yi, y) @@ -95,6 +89,83 @@ class RegularHessianFactor: public HessianFactor { } } + void multiplyHessianAdd(double alpha, const double* x, double* yvalues, + std::vector offsets) const { + + // Use eigen magic to access raw memory + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + + // Create a vector of temporary y values, corresponding to rows i + std::vector y; + y.reserve(size()); + for (const_iterator it = begin(); it != end(); it++) + y.push_back(zero(getDim(it))); + + // Accessing the VectorValues one by one is expensive + // So we will loop over columns to access x only once per column + // And fill the above temporary y values, to be added into yvalues after + for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) { + DenseIndex i = 0; + for (; i < j; ++i) + y[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // blocks on the diagonal are only half + y[i] += info_(j, j).selfadjointView() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + // for below diagonal, we take transpose block from upper triangular part + for (i = j + 1; i < (DenseIndex) size(); ++i) + y[i] += info_(i, j).knownOffDiagonal() + * ConstDMap(x + offsets[keys_[j]], + offsets[keys_[j] + 1] - offsets[keys_[j]]); + } + + // copy to yvalues + for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) + DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) += + alpha * y[i]; + } + + /** Return the diagonal of the Hessian for this factor (raw memory version) */ + virtual void hessianDiagonal(double* d) const { + + // Use eigen magic to access raw memory + //typedef Eigen::Matrix DVector; + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + const Matrix& B = info_(pos, pos).selfadjointView(); + //DMap(d + 9 * j) += B.diagonal(); + DMap(d + D * j) += B.diagonal(); + } + } + + /* ************************************************************************* */ + // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n + virtual void gradientAtZero(double* d) const { + + // Use eigen magic to access raw memory + //typedef Eigen::Matrix DVector; + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + + // Loop over all variables in the factor + for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) { + Key j = keys_[pos]; + // Get the diagonal block, and insert its diagonal + VectorD dj = -info_(pos,size()).knownOffDiagonal(); + //DMap(d + 9 * j) += dj; + DMap(d + D * j) += dj; + } + } + }; } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index ecf8adfec3..75b2d44bad 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -159,7 +159,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { * @brief add the contribution of this factor to the diagonal of the hessian * d(output) = d(input) + deltaHessianFactor */ - void hessianDiagonal(double* d) const { + virtual void hessianDiagonal(double* d) const { // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -434,7 +434,7 @@ class RegularImplicitSchurFactor: public GaussianFactor { /** * Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS */ - void gradientAtZero(double* d) const { + virtual void gradientAtZero(double* d) const { // Use eigen magic to access raw memory typedef Eigen::Matrix DVector; @@ -453,6 +453,12 @@ class RegularImplicitSchurFactor: public GaussianFactor { } } + /// 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/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h new file mode 100644 index 0000000000..bb275ef3f7 --- /dev/null +++ b/gtsam/slam/RegularJacobianFactor.h @@ -0,0 +1,174 @@ +/* ---------------------------------------------------------------------------- + + * 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 RegularJacobianFactor.h + * @brief JacobianFactor class with fixed sized blcoks + * @author Sungtae An + * @date Nov 11, 2014 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +template +class RegularJacobianFactor: public JacobianFactor { + +private: + + /** Use eigen magic to access raw memory */ + typedef Eigen::Matrix DVector; + typedef Eigen::Map DMap; + typedef Eigen::Map ConstDMap; + +public: + + /** 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 + RegularJacobianFactor(const TERMS& terms, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) : + JacobianFactor(terms, b, model) { + } + + /** Constructor with arbitrary number keys, and where the augmented matrix is given all together + * instead of in block terms. Note that only the active view of the provided augmented matrix + * is used, and that the matrix data is copied into a newly-allocated matrix in the constructed + * factor. */ + template + RegularJacobianFactor(const KEYS& keys, + const VerticalBlockMatrix& augmentedMatrix, + const SharedDiagonal& sigmas = SharedDiagonal()) : + JacobianFactor(keys, augmentedMatrix, sigmas) { + } + + /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * Note: this is not assuming a fixed dimension for the variables, + * but requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! */ + void multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const { + + /// Use eigen magic to access raw memory + typedef Eigen::Matrix VectorD; + typedef Eigen::Map MapD; + typedef Eigen::Map ConstMapD; + + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + /// Just iterate over all A matrices and multiply in correct config part (looping over keys) + /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + for (size_t pos = 0; pos < size(); ++pos) + { + Ax += Ab_(pos) + * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]); + } + /// Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + /// multiply with alpha + Ax *= alpha; + + /// Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos){ + MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_( + pos).transpose() * Ax; + } + } + + /** Raw memory access version of multiplyHessianAdd */ + void multiplyHessianAdd(double alpha, const double* x, double* y) const { + + if (empty()) return; + Vector Ax = zero(Ab_.rows()); + + // Just iterate over all A matrices and multiply in correct config part + for(size_t pos=0; poswhitenInPlace(Ax); model_->whitenInPlace(Ax); } + + // multiply with alpha + Ax *= alpha; + + // Again iterate over all A matrices and insert Ai^e into y + for(size_t pos=0; poswhiten(column_k); + dj(k) = dot(column_k, column_k); + }else{ + dj(k) = Ab_(j).col(k).squaredNorm(); + } + } + DMap(d + D * j) += dj; + } + } + + /** Raw memory access version of gradientAtZero + * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n + */ + virtual void gradientAtZero(double* d) const { + + // Get vector b not weighted + Vector b = getb(); + + // Whitening b + if (model_) { + b = model_->whiten(b); + b = model_->whiten(b); + } + + // Just iterate over all A matrices + for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + DVector dj; + // gradient -= A'*b/sigma^2 + // Computing with each column + for (size_t k = 0; k < D; ++k){ + Vector column_k = Ab_(j).col(k); + dj(k) = -1.0*dot(b, column_k); + } + DMap(d + D * j) += dj; + } + } + +}; + +} + diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index d9e7b98191..44d6902fa8 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -48,7 +48,7 @@ class SmartFactorBase: public NonlinearFactor { 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 75e4699d9b..2bfcbfaa0c 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -104,7 +104,7 @@ class SmartProjectionFactor: public SmartFactorBase 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 3b2e2bcbc2..0801d141fd 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -215,4 +215,8 @@ class SmartProjectionPoseFactor: public SmartProjectionFactor +struct traits > : public Testable > {}; + } // \ namespace gtsam diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index f54ee47ba0..7b21e044f7 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -180,4 +180,8 @@ class GenericStereoFactor: public NoiseModelFactor2 { } }; +/// traits +template +struct traits > : public Testable > {}; + } // \ namespace gtsam diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/slam/TriangulationFactor.h similarity index 98% rename from gtsam/geometry/TriangulationFactor.h rename to gtsam/slam/TriangulationFactor.h index fc8d546d39..b7f6a20dce 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/slam/TriangulationFactor.h @@ -10,14 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * testTriangulationFactor.h + * triangulationFactor.h * @date March 2, 2014 * @author Frank Dellaert */ #include #include -#include #include #include diff --git a/gtsam_unstable/slam/expressions.h b/gtsam/slam/expressions.h similarity index 65% rename from gtsam_unstable/slam/expressions.h rename to gtsam/slam/expressions.h index 009be46a19..1d9ddd6d4c 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -7,7 +7,7 @@ #pragma once -#include +#include #include #include @@ -20,11 +20,7 @@ typedef Expression Rot2_; typedef Expression Pose2_; Point2_ transform_to(const Pose2_& x, const Point2_& p) { - Point2(Pose2::*transform)(const Point2& p, - boost::optional H1, - boost::optional H2) const = &Pose2::transform_to; - - return Point2_(x, transform, p); + return Point2_(x, &Pose2::transform_to, p); } // 3D Geometry @@ -34,12 +30,7 @@ typedef Expression Rot3_; typedef Expression Pose3_; Point3_ transform_to(const Pose3_& x, const Point3_& p) { - - Point3(Pose3::*transform)(const Point3& p, - boost::optional Dpose, - boost::optional Dpoint) const = &Pose3::transform_to; - - return Point3_(x, transform, p); + return Point3_(x, &Pose3::transform_to, p); } // Projection @@ -51,8 +42,7 @@ Point2_ project(const Point3_& p_cam) { } Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, - boost::optional Dpose, boost::optional Dpoint, - boost::optional Dcal) { + OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index 5d73bcab81..1dc8975998 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 b227630999..adb759dc07 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 4c0edf5c95..31b2766425 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 e8acb0db07..e0e26ecff3 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 8adbaa62da..240b19a464 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 9cc634b37c..df56f5260e 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 9165ff17a6..db04a74eb2 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 f7bd412fea..2f39701f74 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/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 6bffa3ce95..a8455d6857 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -34,14 +34,30 @@ static SharedNoiseModel model(noiseModel::Unit::Create(1)); typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; +typedef RangeFactorWithTransform RangeFactorWithTransform2D; +typedef RangeFactorWithTransform RangeFactorWithTransform3D; /* ************************************************************************* */ -Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { +Vector factorError2D(const Pose2& pose, const Point2& point, + const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } /* ************************************************************************* */ -Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { +Vector factorError3D(const Pose3& pose, const Point3& point, + const RangeFactor3D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point, + const RangeFactorWithTransform2D& factor) { + return factor.evaluateError(pose, point); +} + +/* ************************************************************************* */ +Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point, + const RangeFactorWithTransform3D& factor) { return factor.evaluateError(pose, point); } @@ -61,10 +77,13 @@ TEST( RangeFactor, ConstructorWithTransform) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model, + body_P_sensor_3D); } /* ************************************************************************* */ @@ -90,14 +109,19 @@ TEST( RangeFactor, EqualsWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2); - Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); - RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D); - RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model, + body_P_sensor_2D); + RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model, + body_P_sensor_2D); CHECK(assert_equal(factor2D_1, factor2D_2)); - RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D); - RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model, + body_P_sensor_3D); + RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model, + body_P_sensor_3D); CHECK(assert_equal(factor3D_1, factor3D_2)); } @@ -130,7 +154,8 @@ TEST( RangeFactor, Error2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -176,8 +201,10 @@ TEST( RangeFactor, Error3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -213,8 +240,10 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -228,7 +257,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { Key pointKey(2); double measurement(10.0); Pose2 body_P_sensor(0.25, -0.10, -M_PI_2); - RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor); + RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot2 R(0.57); @@ -242,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -268,8 +300,10 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -282,8 +316,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { Key poseKey(1); Key pointKey(2); double measurement(10.0); - Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); - RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor); + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); + RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model, + body_P_sensor); // Set the linearization point Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75); @@ -297,8 +333,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose); + H2Expected = numericalDerivative11( + boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -306,6 +344,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRegularHessianFactor.cpp b/gtsam/slam/tests/testRegularHessianFactor.cpp new file mode 100644 index 0000000000..8dfb753f41 --- /dev/null +++ b/gtsam/slam/tests/testRegularHessianFactor.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 testRegularHessianFactor.cpp + * @author Frank Dellaert + * @date March 4, 2014 + */ + +#include +#include + +//#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +const double tol = 1e-5; + +/* ************************************************************************* */ +TEST(RegularHessianFactor, ConstructorNWay) +{ + Matrix G11 = (Matrix(2,2) << 111, 112, 113, 114).finished(); + Matrix G12 = (Matrix(2,2) << 121, 122, 123, 124).finished(); + Matrix G13 = (Matrix(2,2) << 131, 132, 133, 134).finished(); + + Matrix G22 = (Matrix(2,2) << 221, 222, 222, 224).finished(); + Matrix G23 = (Matrix(2,2) << 231, 232, 233, 234).finished(); + + Matrix G33 = (Matrix(2,2) << 331, 332, 332, 334).finished(); + + Vector g1 = (Vector(2) << -7, -9).finished(); + Vector g2 = (Vector(2) << -9, 1).finished(); + Vector g3 = (Vector(2) << 2, 3).finished(); + + double f = 10; + + std::vector js; + js.push_back(0); js.push_back(1); js.push_back(3); + std::vector Gs; + Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33); + std::vector gs; + gs.push_back(g1); gs.push_back(g2); gs.push_back(g3); + RegularHessianFactor<2> factor(js, Gs, gs, f); + + // multiplyHessianAdd: + { + // brute force + Matrix AtA = factor.information(); + HessianFactor::const_iterator i1 = factor.begin(); + HessianFactor::const_iterator i2 = i1 + 1; + Vector X(6); X << 1,2,3,4,5,6; + Vector Y(6); Y << 2633, 2674, 4465, 4501, 5669, 5696; + EXPECT(assert_equal(Y,AtA*X)); + + VectorValues x = map_list_of + (0, (Vector(2) << 1,2).finished()) + (1, (Vector(2) << 3,4).finished()) + (3, (Vector(2) << 5,6).finished()); + + VectorValues expected; + expected.insert(0, Y.segment<2>(0)); + expected.insert(1, Y.segment<2>(2)); + expected.insert(3, Y.segment<2>(4)); + + // RAW ACCESS + Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696; + Vector fast_y = gtsam::zero(8); + double xvalues[8] = {1,2,3,4,0,0,5,6}; + factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + EXPECT(assert_equal(expected_y, fast_y)); + + // now, do it with non-zero y + factor.multiplyHessianAdd(1, xvalues, fast_y.data()); + EXPECT(assert_equal(2*expected_y, fast_y)); + + // check some expressions + EXPECT(assert_equal(G12,factor.info(i1,i2).knownOffDiagonal())); + EXPECT(assert_equal(G22,factor.info(i2,i2).selfadjointView())); + EXPECT(assert_equal((Matrix)G12.transpose(),factor.info(i2,i1).knownOffDiagonal())); + } +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRegularJacobianFactor.cpp b/gtsam/slam/tests/testRegularJacobianFactor.cpp new file mode 100644 index 0000000000..b56b65d7b3 --- /dev/null +++ b/gtsam/slam/tests/testRegularJacobianFactor.cpp @@ -0,0 +1,224 @@ +/** + * @file testRegularJacobianFactor.cpp + * @brief unit test regular jacobian factors + * @author Sungtae An + * @date Nov 12, 2014 + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +static const size_t fixedDim = 3; +static const size_t nrKeys = 3; + +// Keys are assumed to be from 0 to n +namespace { + namespace simple { + // Terms we'll use + const vector > terms = list_of > + (make_pair(0, Matrix3::Identity())) + (make_pair(1, 2*Matrix3::Identity())) + (make_pair(2, 3*Matrix3::Identity())); + + // RHS and sigmas + const Vector b = (Vector(3) << 1., 2., 3.).finished(); + const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5).finished()); + } + + namespace simple2 { + // Terms + const vector > terms2 = list_of > + (make_pair(0, 2*Matrix3::Identity())) + (make_pair(1, 4*Matrix3::Identity())) + (make_pair(2, 6*Matrix3::Identity())); + + // RHS + const Vector b2 = (Vector(3) << 2., 4., 6.).finished(); + } +} + +/* ************************************************************************* */ +// Convert from double* to VectorValues +VectorValues double2vv(const double* x, + const size_t nrKeys, const size_t dim) { + // create map with dimensions + std::map dims; + for (size_t i = 0; i < nrKeys; i++) + dims.insert(make_pair(i, dim)); + + size_t n = nrKeys*dim; + Vector xVec(n); + for (size_t i = 0; i < n; i++){ + xVec(i) = x[i]; + } + return VectorValues(xVec, dims); +} + +/* ************************************************************************* */ +void vv2double(const VectorValues& vv, double* y, + const size_t nrKeys, const size_t dim) { + // create map with dimensions + std::map dims; + for (size_t i = 0; i < nrKeys; i++) + dims.insert(make_pair(i, dim)); + + Vector yvector = vv.vector(dims); + size_t n = nrKeys*dim; + for (size_t j = 0; j < n; j++) + y[j] = yvector[j]; +} + +/* ************************************************************************* */ +TEST(RegularJacobianFactor, constructorNway) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + LONGS_EQUAL((long)terms[2].first, (long)regularFactor.keys().back()); + EXPECT(assert_equal(terms[2].second, regularFactor.getA(regularFactor.end() - 1))); + EXPECT(assert_equal(b, factor.getb())); + EXPECT(assert_equal(b, regularFactor.getb())); + EXPECT(noise == factor.get_model()); + EXPECT(noise == regularFactor.get_model()); +} + +/* ************************************************************************* */ +TEST(RegularJacobianFactor, hessianDiagonal) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + // we compute hessian diagonal from the standard Jacobian + VectorValues expectedHessianDiagonal = factor.hessianDiagonal(); + + // we compare against the Raw memory access implementation of hessianDiagonal + double actualValue[9]={0}; + regularFactor.hessianDiagonal(actualValue); + VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw)); +} + +/* ************************************************************************* */ +TEST(RegularJacobian, gradientAtZero) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + // we compute gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero = factor.gradientAtZero(); + + //EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero())); + + // we compare against the Raw memory access implementation of gradientAtZero + double actualValue[9]={0}; + regularFactor.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); +} + +/* ************************************************************************* */ +TEST(RegularJacobian, gradientAtZero_multiFactors) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + // we compute gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero = factor.gradientAtZero(); + + // we compare against the Raw memory access implementation of gradientAtZero + double actualValue[9]={0}; + regularFactor.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw)); + + // One more factor + using namespace simple2; + JacobianFactor factor2(terms2[0].first, terms2[0].second, + terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second, b2, noise); + RegularJacobianFactor regularFactor2(terms2, b2, noise); + + // we accumulate computed gradient at zero from the standard Jacobian + VectorValues expectedGradientAtZero2 = expectedGradientAtZero.add(factor2.gradientAtZero()); + + // we compare against the Raw memory access implementation of gradientAtZero + regularFactor2.gradientAtZero(actualValue); + VectorValues actualGradientAtZeroRaw2 = double2vv(actualValue,nrKeys,fixedDim); + EXPECT(assert_equal(expectedGradientAtZero2, actualGradientAtZeroRaw2)); + +} + +/* ************************************************************************* */ +TEST(RegularJacobian, multiplyHessianAdd) +{ + using namespace simple; + JacobianFactor factor(terms[0].first, terms[0].second, + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise); + RegularJacobianFactor regularFactor(terms, b, noise); + + // arbitrary vector X + VectorValues X; + X.insert(0, (Vector(3) << 10.,20.,30.).finished()); + X.insert(1, (Vector(3) << 10.,20.,30.).finished()); + X.insert(2, (Vector(3) << 10.,20.,30.).finished()); + + // arbitrary vector Y + VectorValues Y; + Y.insert(0, (Vector(3) << 10.,10.,10.).finished()); + Y.insert(1, (Vector(3) << 20.,20.,20.).finished()); + Y.insert(2, (Vector(3) << 30.,30.,30.).finished()); + + // multiplyHessianAdd Y += alpha*A'A*X + double alpha = 2.0; + VectorValues expectedMHA = Y; + factor.multiplyHessianAdd(alpha, X, expectedMHA); + + // create data for raw memory access + double XRaw[9]; + vv2double(X, XRaw, nrKeys, fixedDim); + + // test 1st version: multiplyHessianAdd(double alpha, const double* x, double* y) + double actualMHARaw[9]; + vv2double(Y, actualMHARaw, nrKeys, fixedDim); + regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw); + VectorValues actualMHARawVV = double2vv(actualMHARaw,nrKeys,fixedDim); + EXPECT(assert_equal(expectedMHA,actualMHARawVV)); + + // test 2nd version: multiplyHessianAdd(double alpha, const double* x, double* y, std::vector keys) + double actualMHARaw2[9]; + vv2double(Y, actualMHARaw2, nrKeys, fixedDim); + vector dims; + size_t accumulatedDim = 0; + for (size_t i = 0; i < nrKeys+1; i++){ + dims.push_back(accumulatedDim); + accumulatedDim += fixedDim; + } + regularFactor.multiplyHessianAdd(alpha, XRaw, actualMHARaw2, dims); + VectorValues actualMHARawVV2 = double2vv(actualMHARaw2,nrKeys,fixedDim); + EXPECT(assert_equal(expectedMHA,actualMHARawVV2)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 2a2e3b40fd..9c99628e68 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/slam/tests/testTriangulationFactor.cpp b/gtsam/slam/tests/testTriangulationFactor.cpp new file mode 100644 index 0000000000..6b79171df6 --- /dev/null +++ b/gtsam/slam/tests/testTriangulationFactor.cpp @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/** + * testTriangulationFactor.cpp + * + * Created on: July 30th, 2013 + * Author: cbeall3 + */ + +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +// Some common constants +static const boost::shared_ptr sharedCal = // + boost::make_shared(1500, 1200, 0, 640, 480); + +// Looking along X-axis, 1 meter above ground plane (x-y) +static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2); +static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1)); +PinholeCamera camera1(pose1, *sharedCal); + +// landmark ~5 meters infront of camera +static const Point3 landmark(5, 0.5, 1.2); + +// 1. Project two landmarks into two cameras and triangulate +Point2 z1 = camera1.project(landmark); + +//****************************************************************************** +TEST( triangulation, TriangulationFactor ) { + // Create the factor with a measurement that is 3 pixels off in x + Key pointKey(1); + SharedNoiseModel model; + typedef TriangulationFactor<> Factor; + Factor factor(camera1, z1, model, pointKey); + + // Use the factor to calculate the Jacobians + Matrix HActual; + factor.evaluateError(landmark, HActual); + + Matrix HExpected = numericalDerivative11( + boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); + + // Verify the Jacobians are correct + CHECK(assert_equal(HExpected, HActual, 1e-3)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 2e59a98a42..5dda02350b 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 526120a300..cac7110434 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 01f7366d61..4b89a4b603 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 5a44d451db..e20fb67bd5 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 0054f9fbce..f30f889353 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 37aae24002..9813df3313 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 cd42b8d4f8..6abfe43365 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 1c2807e7a8..66538e8021 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -100,7 +100,7 @@ class FullIMUFactor : public NoiseModelFactor2 { 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 73fc58fa6a..5ed079acb2 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -90,7 +90,7 @@ class IMUFactor : public NoiseModelFactor2 { 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 06aa4ac24f..818266d4cc 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 43d0187526..ea7a2c9ffe 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -25,7 +25,11 @@ class GTSAM_UNSTABLE_EXPORT PoseRTV { 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 @@ class GTSAM_UNSTABLE_EXPORT PoseRTV { * - 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 @@ class GTSAM_UNSTABLE_EXPORT PoseRTV { * 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 @@ class GTSAM_UNSTABLE_EXPORT PoseRTV { } }; -// Define GTSAM traits -namespace traits { - -template<> -struct is_manifold : public boost::true_type { -}; - -template<> -struct dimension : public boost::integral_constant { -}; template<> -struct zero { - static PoseRTV value() { - return PoseRTV(); - } -}; +struct traits : public internal::LieGroupTraits {}; -} } // \namespace gtsam diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index b1c254a9ea..a3dd6327b2 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 @@ class Reconstruction : public NoiseModelFactor3 { 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); - 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; + 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_; } - return Pose3::Logmap(hx); + return error; } }; @@ -92,7 +92,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3( - 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 7e48023937..56d38714a2 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 4c84bbe564..3c9247048a 100644 --- a/gtsam_unstable/geometry/BearingS2.h +++ b/gtsam_unstable/geometry/BearingS2.h @@ -99,4 +99,7 @@ class GTSAM_UNSTABLE_EXPORT BearingS2 { }; +/// traits +template<> struct traits : public Testable {}; + } // \namespace gtsam diff --git a/gtsam_unstable/geometry/Event.h b/gtsam_unstable/geometry/Event.h new file mode 100644 index 0000000000..9d35331bb2 --- /dev/null +++ b/gtsam_unstable/geometry/Event.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 Event + * @brief Space-time event + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/// A space-time event +class Event { + + double time_; ///< Time event was generated + Point3 location_; ///< Location at time event was generated + +public: + enum { dimension = 4 }; + + /// Speed of sound + static const double Speed; + static const Matrix14 JacobianZ; + + /// Default Constructor + Event() : + time_(0) { + } + + /// Constructor from time and location + Event(double t, const Point3& p) : + time_(t), location_(p) { + } + + /// Constructor with doubles + Event(double t, double x, double y, double z) : + time_(t), location_(x, y, z) { + } + + double time() const { return time_;} + Point3 location() const { return location_;} + + // TODO we really have to think of a better way to do linear arguments + double height(OptionalJacobian<1,4> H = boost::none) const { + if (H) *H = JacobianZ; + return location_.z(); + } + + /** print with optional string */ + void print(const std::string& s = "") const { + std::cout << s << "time = " << time_; + location_.print("; location"); + } + + /** equals with an tolerance */ + bool equals(const Event& other, double tol = 1e-9) const { + return std::abs(time_ - other.time_) < tol + && location_.equals(other.location_, tol); + } + + /// Updates a with tangent space delta + inline Event retract(const Vector4& v) const { + return Event(time_ + v[0], location_.retract(v.tail(3))); + } + + /// Returns inverse retraction + inline Vector4 localCoordinates(const Event& q) const { + return Vector4::Zero(); // TODO + } + + /// Time of arrival to given microphone + double toa(const Point3& microphone, // + OptionalJacobian<1, 4> H1 = boost::none, // + OptionalJacobian<1, 3> H2 = boost::none) const { + Matrix13 D1, D2; + double distance = location_.distance(microphone, D1, D2); + if (H1) + // derivative of toa with respect to event + *H1 << 1.0, D1 / Speed; + if (H2) + // derivative of toa with respect to microphone location + *H2 << D2 / Speed; + return time_ + distance / Speed; + } +}; + +const double Event::Speed = 330; +const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished(); + +// Define GTSAM traits +template<> +struct GTSAM_EXPORT traits : internal::Manifold {}; + +} //\ namespace gtsam diff --git a/gtsam_unstable/geometry/Pose3Upright.h b/gtsam_unstable/geometry/Pose3Upright.h index 22aab5d44f..c48508fa0c 100644 --- a/gtsam_unstable/geometry/Pose3Upright.h +++ b/gtsam_unstable/geometry/Pose3Upright.h @@ -140,18 +140,8 @@ class GTSAM_UNSTABLE_EXPORT Pose3Upright { }; // \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 eb8ddace60..9087cac2ac 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 cf2142af8a..38bba2ee31 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/geometry/tests/testEvent.cpp b/gtsam_unstable/geometry/tests/testEvent.cpp new file mode 100644 index 0000000000..433ca7e7f8 --- /dev/null +++ b/gtsam_unstable/geometry/tests/testEvent.cpp @@ -0,0 +1,100 @@ +/* ---------------------------------------------------------------------------- + + * 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 testEvent.cpp + * @brief Unit tests for space time "Event" + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// Create a noise model for the TOA error +static const double ms = 1e-3; +static const double cm = 1e-2; +typedef Eigen::Matrix Vector1; +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1,0.5*ms)); + +static const double timeOfEvent = 25; +static const Event exampleEvent(timeOfEvent, 1, 0, 0); +static const Point3 microphoneAt0; + +//***************************************************************************** +TEST( Event, Constructor ) { + const double t = 0; + Event actual(t, 201.5 * cm, 201.5 * cm, (212 - 45) * cm); +} + +//***************************************************************************** +TEST( Event, Toa1 ) { + Event event(0, 1, 0, 0); + double expected = 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expected, event.toa(microphoneAt0), 1e-9); +} + +//***************************************************************************** +TEST( Event, Toa2 ) { + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, exampleEvent.toa(microphoneAt0), 1e-9); +} + +//************************************************************************* +TEST (Event, Derivatives) { + Matrix14 actualH1; + Matrix13 actualH2; + exampleEvent.toa(microphoneAt0, actualH1, actualH2); + Matrix expectedH1 = numericalDerivative11( + boost::bind(&Event::toa, _1, microphoneAt0, boost::none, boost::none), + exampleEvent); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&Event::toa, exampleEvent, _1, boost::none, boost::none), + microphoneAt0); + EXPECT(assert_equal(expectedH2, actualH2, 1e-8)); +} + +//***************************************************************************** +TEST( Event, Expression ) { + Key key = 12; + Expression event_(key); + Expression knownMicrophone_(microphoneAt0); // constant expression + Expression expression(&Event::toa, event_, knownMicrophone_); + + Values values; + values.insert(key, exampleEvent); + double expectedTOA = timeOfEvent + 1 / Event::Speed; + EXPECT_DOUBLES_EQUAL(expectedTOA, expression.value(values), 1e-9); +} + +//***************************************************************************** +TEST(Event, Retract) { + Event event, expected(1, 2, 3, 4); + Vector4 v; + v << 1, 2, 3, 4; + EXPECT(assert_equal(expected, event.retract(v))); +} + +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** + diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index 26a469c924..06ac9ab80d 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -23,42 +23,55 @@ namespace gtsam { /** * 3D similarity transform */ -class Similarity3: private Matrix4 { +class Similarity3: public LieGroup { + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; + +private: + Rot3 R_; + Point3 t_; + double s_; + +public: /// Construct from Eigen types Similarity3(const Matrix3& R, const Vector3& t, double s) { - *this << s * R, t, 0, 0, 0, 1; + R_ = R; + t_ = t; + s_ = s; } /// Return the translation - const Eigen::Block t() const { - return this->topRightCorner<3, 1>(); + const Vector3 t() const { + return t_.vector(); } /// Return the rotation matrix - const Eigen::Block R() const { - return this->topLeftCorner<3, 3>(); + const Matrix3 R() const { + return R_.matrix(); } public: - Similarity3() { - setIdentity(); + Similarity3() : + R_(), t_(), s_(1){ } /// Construct pure scaling Similarity3(double s) { - setIdentity(); - this->topLeftCorner<3, 3>() = s * Matrix3::Identity(); + s_ = s; } /// Construct from GTSAM types Similarity3(const Rot3& R, const Point3& t, double s) { - *this << R.matrix(), t.vector(), 0, 0, 0, 1.0 / s; + R_ = R; + t_ = t; + s_ = s; } bool operator==(const Similarity3& other) const { - return Matrix4::operator==(other); + return (R_.equals(other.R_)) && (t_ == other.t_) && (s_ == other.s_); } /// Compare with tolerance @@ -67,6 +80,39 @@ class Similarity3: private Matrix4 { && scale() < (sim.scale()+tol) && scale() > (sim.scale()-tol); } + Point3 transform_from(const Point3& p) const { + /*if (Dpose) { + const Matrix3 R = R_.matrix(); + Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + (*Dpose) << DR, R; + } + if (Dpoint) + *Dpoint = R_.matrix();*/ + return R_ * (s_ * p) + t_; + } + + Matrix7 adjointMap() const{ + const Matrix3 R = R_.matrix(); + const Vector3 t = t_.vector(); + Matrix3 A = s_ * skewSymmetric(t) * R; + Matrix7 adj; + adj << s_*R, A, -s_*t, Z_3x3, R, Eigen::Matrix::Zero(), Z_3x3, Z_3x3, 1; + } + + /** syntactic sugar for transform_from */ + inline Point3 operator*(const Point3& p) const { + return transform_from(p); + } + + /*Similarity3 inverse() const { + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt * (-t_)); + }*/ + + Similarity3 operator*(const Similarity3& T) const { + return Similarity3(R_ * T.R_, ((1.0/s_)*t_) + R_ * T.t_, s_*T.s_); + } + void print(const std::string& s) const { std::cout << s; rotation().print("R:\n"); @@ -82,35 +128,34 @@ class Similarity3: private Matrix4 { return 7; } - /// Dimensionality of tangent space = 3 DOF + /// Dimensionality of tangent space = 7 DOF inline size_t dim() const { return 7; } /// Return the rotation matrix Rot3 rotation() const { - return R().eval(); + return R_; } /// Return the translation Point3 translation() const { - Vector3 t = this->t(); - return Point3::Expmap(t); + return t_; } /// Return the scale double scale() const { - return 1.0 / (*this)(3, 3); + return s_; } /// Update Similarity transform via 7-dim vector in tangent space - Similarity3 retract(const Vector7& v) const { +/* Similarity3 retract(const Vector7& v) const { // Will retracting or localCoordinating R work if R is not a unit rotation? // Also, how do we actually get s out? Seems like we need to store it somewhere. return Similarity3( // - rotation().retract(v.head<3>()), // retract rotation using v[0,1,2] - translation().retract(R() * v.segment<3>(3)), // Retract the translation + R_.retract(v.head<3>()), // retract rotation using v[0,1,2] + t_.retract(R() * v.segment<3>(3)), // Retract the translation scale() + v[6]); //finally, update scale using v[6] } @@ -118,12 +163,12 @@ class Similarity3: private Matrix4 { Vector7 localCoordinates(const Similarity3& other) const { Vector7 v; - v.head<3>() = rotation().localCoordinates(other.rotation()); - v.segment<3>(3) = rotation().unrotate(other.translation() - translation()).vector(); + v.head<3>() = R_.localCoordinates(other.R_); + v.segment<3>(3) = R_.unrotate(other.t_ - t_).vector(); //v.segment<3>(3) = translation().localCoordinates(other.translation()); - v[6] = other.scale() - scale(); + v[6] = other.s_ - s_; return v; - } + }*/ /// @} @@ -138,6 +183,9 @@ class Similarity3: private Matrix4 { /// @} }; + +template<> +struct traits : public internal::LieGroupTraits {}; } #include @@ -152,6 +200,8 @@ class Similarity3: private Matrix4 { using namespace gtsam; using namespace std; +//GTSAM_CONCEPT_POSE_INST(Similarity3); + static Point3 P(0.2,0.7,-2); static Rot3 R = Rot3::rodriguez(0.3,0,0); static Similarity3 T(R,Point3(3.5,-8.2,4.2),1); @@ -252,6 +302,7 @@ TEST(Similarity3, Optimization) { prior.print("goal angle"); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1); Symbol key('x',1); + Symbol key2('x',2); PriorFactor factor(key, prior, model); NonlinearFactorGraph graph; @@ -259,13 +310,13 @@ TEST(Similarity3, Optimization) { graph.print("full graph"); Values initial; - initial.insert(key, Similarity3()); + initial.insert(key, Similarity3()); initial.print("initial estimate"); - Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + Values result; + result.insert(key2, LevenbergMarquardtOptimizer(graph, initial).optimize()); result.print("final result"); - - EXPECT(assert_equal(prior, result, 1e-2)); + EXPECT(assert_equal(prior, result.at(key2), 1e-2)); } diff --git a/gtsam_unstable/linear/CMakeLists.txt b/gtsam_unstable/linear/CMakeLists.txt new file mode 100644 index 0000000000..99a4b814e8 --- /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 0000000000..bc1b2bc125 --- /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 0000000000..9c067ae3df --- /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 0000000000..7c62c3d54c --- /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 0000000000..eca271941a --- /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 0000000000..111ab506fe --- /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 0000000000..f0eb7d7fb4 --- /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 0000000000..f7a575f8cd --- /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 0000000000..43df23daaf --- /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 0000000000..bf41743a2b --- /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 0000000000..8fca61ca49 --- /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 9b2903a144..5c66d411f3 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.h @@ -67,7 +67,7 @@ class GTSAM_UNSTABLE_EXPORT BatchFixedLagSmoother : public FixedLagSmoother { 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 fce28b2140..9a458ee5a5 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 d85109605d..f3808686c8 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h @@ -204,4 +204,9 @@ class GTSAM_UNSTABLE_EXPORT ConcurrentBatchSmoother : public ConcurrentSmoother /// 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 4f2e1b0aa2..11012674e5 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h @@ -198,4 +198,9 @@ class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalFilter : public virtual Concurr /// 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 5fe6effb29..78bab50791 100644 --- a/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h +++ b/gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h @@ -168,4 +168,9 @@ class GTSAM_UNSTABLE_EXPORT ConcurrentIncrementalSmoother : public virtual Concu /// 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 7be2191879..72c7c66f56 100644 --- a/gtsam_unstable/nonlinear/LinearizedFactor.h +++ b/gtsam_unstable/nonlinear/LinearizedFactor.h @@ -156,8 +156,10 @@ class GTSAM_UNSTABLE_EXPORT LinearizedJacobianFactor : public LinearizedGaussian } }; - - +/// traits +template<> +struct traits : public Testable { +}; /** * A factor that takes a linear, Hessian factor and inserts it into @@ -269,6 +271,9 @@ class GTSAM_UNSTABLE_EXPORT LinearizedHessianFactor : public LinearizedGaussianF } }; - +/// traits +template<> +struct traits : public Testable { +}; } // \namespace aspn diff --git a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp b/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp deleted file mode 100644 index 5d6ea21688..0000000000 --- a/gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp +++ /dev/null @@ -1,231 +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 testBasisDecompositions.cpp - * @date November 23, 2014 - * @author Frank Dellaert - * @brief unit tests for Basis Decompositions w Expressions - */ - -#include - -namespace gtsam { - -/// Fourier -template -class Fourier { - -public: - - typedef Eigen::Matrix Coefficients; - typedef Eigen::Matrix Jacobian; - -private: - - double x_; - Jacobian H_; - -public: - - /// Constructor - Fourier(double x) : - x_(x) { - H_(0, 0) = 1; - for (size_t i = 1; i < N; i += 2) { - H_(0, i) = cos(i * x); - H_(0, i + 1) = sin(i * x); - } - } - - /// Given coefficients c, predict value for x - double operator()(const Coefficients& c, boost::optional H) { - if (H) - (*H) = H_; - return H_ * c; - } -}; - -} - -#include -#include -#include -#include - -namespace gtsam { - -/// For now, this is our sequence representation -typedef std::map Sequence; - -/** - * Class that does Fourier Decomposition via least squares - */ -class FourierDecomposition { -public: - - typedef Vector3 Coefficients; ///< Fourier coefficients - -private: - Coefficients c_; - -public: - - /// Create nonlinear FG from Sequence - static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, - const SharedNoiseModel& model) { - NonlinearFactorGraph graph; - Expression c(0); - typedef std::pair Sample; - BOOST_FOREACH(Sample sample, sequence) { - Expression expression(Fourier<3>(sample.first), c); - ExpressionFactor factor(model, sample.second, expression); - graph.add(factor); - } - return graph; - } - - /// Create linear FG from Sequence - static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence& sequence, - const SharedNoiseModel& model) { - NonlinearFactorGraph graph = NonlinearGraph(sequence, model); - Values values; - values.insert(0, Coefficients::Zero()); // does not matter - GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); - return gfg; - } - - /// Constructor - FourierDecomposition(const Sequence& sequence, - const SharedNoiseModel& model) { - GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model); - VectorValues solution = gfg->optimize(); - c_ = solution.at(0); - } - - /// Return Fourier coefficients - Coefficients coefficients() { - return c_; - } -}; - -} - -#include -#include -#include - -using namespace std; -using namespace gtsam; - -noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); - -//****************************************************************************** -TEST(BasisDecompositions, Fourier) { - Fourier<3> fx(0); - Eigen::Matrix expectedH, actualH; - Vector3 c(1.5661, 1.2717, 1.2717); - expectedH = numericalDerivative11( - boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c); - EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9); - EXPECT(assert_equal((Matrix)expectedH, actualH)); -} - -//****************************************************************************** -TEST(BasisDecompositions, ManualFourier) { - - // Create linear factor graph - GaussianFactorGraph g; - Key key(1); - Expression c(key); - Values values; - values.insert(key, Vector3::Zero()); // does not matter - for (size_t i = 0; i < 16; i++) { - double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); - - // Manual JacobianFactor - Matrix A(1, 3); - A << 1, cos(x), sin(x); - Vector b(1); - b << y; - JacobianFactor f1(key, A, b); - g.add(f1); - - // With ExpressionFactor - Expression expression(Fourier<3>(x), c); - EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9); - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - { - ExpressionFactor f2(model, y, expression); - boost::shared_ptr gf = f2.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - CHECK(jf); - EXPECT( assert_equal(f1, *jf, 1e-9)); - } - } - - // Solve - VectorValues actual = g.optimize(); - - // Check - Vector3 expected(1.5661, 1.2717, 1.2717); - EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4)); -} - -//****************************************************************************** -TEST(BasisDecompositions, FourierDecomposition) { - - // Create example sequence - Sequence sequence; - for (size_t i = 0; i < 16; i++) { - double x = i * M_PI / 8, y = exp(sin(x) + cos(x)); - sequence[x] = y; - } - - // Do Fourier Decomposition - FourierDecomposition actual(sequence, model); - - // Check - Vector3 expected(1.5661, 1.2717, 1.2717); - EXPECT(assert_equal((Vector) expected, actual.coefficients(),1e-4)); -} - -//****************************************************************************** -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -//****************************************************************************** - diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index d10e310026..7c2f9d9b94 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 @@ -179,29 +179,29 @@ TEST(ExpressionFactor, InvokeDerivatives) { // Let's assign it it to a boost function object // cast is needed because Pose3::transform_to is overloaded - typedef boost::function F; - F f = static_cast(&Pose3::transform_to); - - // Create arguments -Pose3 pose; - Point3 point; - typedef boost::fusion::vector Arguments; - Arguments args = boost::fusion::make_vector(pose, point); - - // Create fused function (takes fusion vector) and call it - boost::fusion::fused g(f); - Point3 actual = g(args); - CHECK(assert_equal(point,actual)); - - // We can *immediately* do this using invoke - Point3 actual2 = boost::fusion::invoke(f, args); - CHECK(assert_equal(point,actual2)); +// typedef boost::function F; +// F f = static_cast(&Pose3::transform_to); +// +// // Create arguments +// Pose3 pose; +// Point3 point; +// typedef boost::fusion::vector Arguments; +// Arguments args = boost::fusion::make_vector(pose, point); +// +// // Create fused function (takes fusion vector) and call it +// boost::fusion::fused g(f); +// Point3 actual = g(args); +// CHECK(assert_equal(point,actual)); +// +// // We can *immediately* do this using invoke +// Point3 actual2 = boost::fusion::invoke(f, args); +// CHECK(assert_equal(point,actual2)); // Now, let's create the optional Jacobian arguments typedef Point3 T; typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; + typedef boost::mpl::transform >::type Optionals; // Unfortunately this is moot: we need a pointer to a function with the // optional derivatives; I don't see a way of calling a function that we @@ -215,8 +215,8 @@ struct proxy { return pose.transform_to(point); } Point3 operator()(const Pose3& pose, const Point3& point, - boost::optional Dpose, - boost::optional Dpoint) const { + OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { return pose.transform_to(point, Dpose, Dpoint); } }; diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index 2a756b5af1..2a27730f46 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/gtsam_unstable/slam/AHRS.cpp @@ -12,17 +12,14 @@ using namespace std; namespace gtsam { -Matrix cov(const Matrix& m) { +/* ************************************************************************* */ +Matrix3 AHRS::Cov(const Vector3s& m) { const double num_observations = m.cols(); - const Vector mean = m.rowwise().sum() / num_observations; - Matrix D = m.colwise() - mean; - Matrix DDt = D * trans(D); - return DDt / (num_observations - 1); + const Vector3 mean = m.rowwise().sum() / num_observations; + Vector3s D = m.colwise() - mean; + return D * trans(D) / (num_observations - 1); } -Matrix I3 = eye(3); -Matrix Z3 = zeros(3, 3); - /* ************************************************************************* */ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, bool flat) : @@ -34,11 +31,11 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, size_t T = stationaryU.cols(); // estimate standard deviation on gyroscope readings - Pg_ = cov(stationaryU); - Vector sigmas_v_g = esqrt(Pg_.diagonal() * T); + Pg_ = Cov(stationaryU); + Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T); // estimate standard deviation on accelerometer readings - Pa_ = cov(stationaryF); + Pa_ = Cov(stationaryF); // Quantities needed for integration @@ -46,15 +43,13 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e, double tau_g = 730; // correlation time for gyroscope double tau_a = 730; // correlation time for accelerometer - F_g_ = -I3 / tau_g; - F_a_ = -I3 / tau_a; + F_g_ = -I_3x3 / tau_g; + F_a_ = -I_3x3 / tau_a; Vector3 var_omega_w = 0 * ones(3); // TODO Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3); Vector3 var_omega_a = (0.034 * 0.034) * ones(3); - Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g); - Vector vars(12); - vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; - var_w_ = diag(vars); + Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g); + var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a; // Quantities needed for aiding sigmas_v_a_ = esqrt(T * Pa_.diagonal()); @@ -95,15 +90,15 @@ std::pair AHRS::initialize(double g_e) Matrix P_plus_k2 = Matrix(9, 9); P_plus_k2.block<3,3>(0, 0) = P11; - P_plus_k2.block<3,3>(0, 3) = Z3; + P_plus_k2.block<3,3>(0, 3) = Z_3x3; P_plus_k2.block<3,3>(0, 6) = P12; - P_plus_k2.block<3,3>(3, 0) = Z3; + P_plus_k2.block<3,3>(3, 0) = Z_3x3; P_plus_k2.block<3,3>(3, 3) = Pg_; - P_plus_k2.block<3,3>(3, 6) = Z3; + P_plus_k2.block<3,3>(3, 6) = Z_3x3; P_plus_k2.block<3,3>(6, 0) = trans(P12); - P_plus_k2.block<3,3>(6, 3) = Z3; + P_plus_k2.block<3,3>(6, 3) = Z_3x3; P_plus_k2.block<3,3>(6, 6) = Pa; Vector dx = zero(9); @@ -123,26 +118,26 @@ std::pair AHRS::integrate( // FIXME: //if nargout>1 Matrix bRn = mech.bRn().matrix(); - Matrix I3 = eye(3); - Matrix Z3 = zeros(3, 3); - Matrix F_k = zeros(9, 9); + Matrix9 F_k; F_k.setZero(); F_k.block<3,3>(0, 3) = -bRn; F_k.block<3,3>(3, 3) = F_g_; F_k.block<3,3>(6, 6) = F_a_; - Matrix G_k = zeros(9, 12); + typedef Eigen::Matrix Matrix9_12; + Matrix9_12 G_k; G_k.setZero(); G_k.block<3,3>(0, 0) = -bRn; G_k.block<3,3>(0, 6) = -bRn; - G_k.block<3,3>(3, 3) = I3; - G_k.block<3,3>(6, 9) = I3; + G_k.block<3,3>(3, 3) = I_3x3; + G_k.block<3,3>(6, 9) = I_3x3; - Matrix Q_k = G_k * var_w_ * trans(G_k); - Matrix Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix + Matrix9 Q_k = G_k * var_w_.asDiagonal() * G_k.transpose(); + Matrix9 Psi_k = I_9x9 + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix // This implements update in section 10.6 - Matrix B = zeros(9, 9); - Vector u2 = zero(9); + Matrix9 B; B.setZero(); + Vector9 u2; u2.setZero(); + // TODO predictQ should be templated to also take fixed size matrices. KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k); return make_pair(newMech, newState); } @@ -175,7 +170,7 @@ std::pair AHRS::aid( if (Farrell) { // calculate residual gravity measurement z = n_g_ - trans(bRn) * measured_b_g; - H = collect(3, &n_g_cross_, &Z3, &bRn); + H = collect(3, &n_g_cross_, &Z_3x3, &bRn); R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn; } else { // my measurement prediction (in body frame): @@ -189,7 +184,7 @@ std::pair AHRS::aid( z = bRn * n_g_ - measured_b_g; // Now the Jacobian H Matrix b_g = bRn * n_g_cross_; - H = collect(3, &b_g, &Z3, &I3); + H = collect(3, &b_g, &Z_3x3, &I_3x3); // And the measurement noise, TODO: should be created once where sigmas_v_a is given R = diag(emul(sigmas_v_a_, sigmas_v_a_)); } @@ -219,7 +214,7 @@ std::pair AHRS::aidGeneral( Vector z = f - increment * f_previous; //Vector z = increment * f_previous - f; Matrix b_g = skewSymmetric(increment* f_previous); - Matrix H = collect(3, &b_g, &I3, &Z3); + Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3); // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_)); // Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice Matrix R = diag(Vector3(0.01, 0.0001, 0.01)); @@ -240,16 +235,16 @@ std::pair AHRS::aidGeneral( /* ************************************************************************* */ void AHRS::print(const std::string& s) const { mech0_.print(s + ".mech0_"); - gtsam::print(F_g_, s + ".F_g"); - gtsam::print(F_a_, s + ".F_a"); - gtsam::print(var_w_, s + ".var_w"); + gtsam::print((Matrix)F_g_, s + ".F_g"); + gtsam::print((Matrix)F_a_, s + ".F_a"); + gtsam::print((Vector)var_w_, s + ".var_w"); - gtsam::print(sigmas_v_a_, s + ".sigmas_v_a"); - gtsam::print(n_g_, s + ".n_g"); - gtsam::print(n_g_cross_, s + ".n_g_cross"); + gtsam::print((Vector)sigmas_v_a_, s + ".sigmas_v_a"); + gtsam::print((Vector)n_g_, s + ".n_g"); + gtsam::print((Matrix)n_g_cross_, s + ".n_g_cross"); - gtsam::print(Pg_, s + ".P_g"); - gtsam::print(Pa_, s + ".P_a"); + gtsam::print((Matrix)Pg_, s + ".P_g"); + gtsam::print((Matrix)Pa_, s + ".P_a"); } diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h index 81d74a9f5e..e15e6e0f73 100644 --- a/gtsam_unstable/slam/AHRS.h +++ b/gtsam_unstable/slam/AHRS.h @@ -14,8 +14,6 @@ namespace gtsam { -GTSAM_UNSTABLE_EXPORT Matrix cov(const Matrix& m); - class GTSAM_UNSTABLE_EXPORT AHRS { private: @@ -25,18 +23,24 @@ class GTSAM_UNSTABLE_EXPORT AHRS { KalmanFilter KF_; ///< initial Kalman filter // Quantities needed for integration - Matrix F_g_; ///< gyro bias dynamics - Matrix F_a_; ///< acc bias dynamics - Matrix var_w_; ///< dynamic noise variances + Matrix3 F_g_; ///< gyro bias dynamics + Matrix3 F_a_; ///< acc bias dynamics + + typedef Eigen::Matrix Variances; + Variances var_w_; ///< dynamic noise variances // Quantities needed for aiding - Vector sigmas_v_a_; ///< measurement sigma - Vector n_g_; ///< gravity in nav frame - Matrix n_g_cross_; ///< and its skew-symmetric matrix + Vector3 sigmas_v_a_; ///< measurement sigma + Vector3 n_g_; ///< gravity in nav frame + Matrix3 n_g_cross_; ///< and its skew-symmetric matrix - Matrix Pg_, Pa_; + Matrix3 Pg_, Pa_; public: + + typedef Eigen::Matrix Vector3s; + static Matrix3 Cov(const Vector3s& m); + /** * AHRS constructor * @param stationaryU initial interval of gyro measurements, 3xn matrix diff --git a/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h b/gtsam_unstable/slam/GaussMarkov1stOrderFactor.h index 810673b642..857c077618 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 @@ class GaussMarkov1stOrderFactor: public NoiseModelFactor2 { 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 @@ class GaussMarkov1stOrderFactor: public NoiseModelFactor2 { }; // \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 805c0a71a2..6b111b7590 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -391,7 +391,12 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5(*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 c5b1fc2a42..4218a55610 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 dda267a591..fa33ce5b9b 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 @@ class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 { * @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 @@ class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 { } /// 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 @@ class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 { * @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 @@ class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 { * @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 24f0839073..991aae1fd2 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 b69f852b45..0426c3ba42 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 d3bfbbb7df..5906a66647 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 1c0d1bc376..5d2ba33238 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -107,7 +107,7 @@ class SmartStereoProjectionFactor: public SmartFactorBase This; - typedef traits::dimension ZDim_t; ///< Dimension trait of measurement type + enum {ZDim = 3}; ///< Dimension trait of measurement type public: @@ -482,7 +482,7 @@ class SmartStereoProjectionFactor: public SmartFactorBase >(this->keys_); + return boost::make_shared< JacobianFactorSVD >(this->keys_); } /// Returns true if nonDegenerate @@ -745,4 +745,10 @@ class SmartStereoProjectionFactor: public SmartFactorBase +struct traits > : + public Testable > { +}; + } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 1f2bd19425..2e3bc866bd 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -215,4 +215,10 @@ class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor +struct traits > : + public Testable > { +}; + } // \ namespace gtsam diff --git a/gtsam_unstable/slam/TOAFactor.h b/gtsam_unstable/slam/TOAFactor.h new file mode 100644 index 0000000000..b500b36e31 --- /dev/null +++ b/gtsam_unstable/slam/TOAFactor.h @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * 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 TOAFactor.h + * @brief "Time of Arrival" factor + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include + +namespace gtsam { + +/// A "Time of Arrival" factor - so little code seems hardly worth it :-) +class TOAFactor: public ExpressionFactor { + + typedef Expression double_; + +public: + + /** + * Constructor + * @param some expression yielding an event + * @param microphone_ expression yielding a microphone location + * @param toaMeasurement time of arrival at microphone + * @param model noise model + */ + TOAFactor(const Expression& eventExpression, + const Expression& microphone_, double toaMeasurement, + const SharedNoiseModel& model) : + ExpressionFactor(model, toaMeasurement, + double_(&Event::toa, eventExpression, microphone_)) { + } + +}; + +} //\ namespace gtsam + diff --git a/gtsam_unstable/slam/TSAMFactors.h b/gtsam_unstable/slam/TSAMFactors.h index d4c5f81724..aae4e413d5 100644 --- a/gtsam_unstable/slam/TSAMFactors.h +++ b/gtsam_unstable/slam/TSAMFactors.h @@ -49,7 +49,7 @@ class DeltaFactor: public NoiseModelFactor2 { 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 @@ class DeltaFactorBase: public NoiseModelFactor4 { *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 249e4fc20b..2abc49fa17 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 e3de436289..adfb9ae367 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 2f54528b8b..31995e769e 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/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp index 44f516ae9d..d0980f4529 100644 --- a/gtsam_unstable/slam/tests/testAHRS.cpp +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -30,7 +30,7 @@ TEST (AHRS, cov) { 9.0, 4.0, 7.0, 6.0, 3.0, 2.0).finished(); - Matrix actual = cov(trans(A)); + Matrix actual = AHRS::Cov(trans(A)); Matrix expected = (Matrix(3, 3) << 10.9167, 2.3333, 5.0000, 2.3333, 4.6667, -2.6667, @@ -42,7 +42,7 @@ TEST (AHRS, cov) { /* ************************************************************************* */ TEST (AHRS, covU) { - Matrix actual = cov(10000*stationaryU); + Matrix actual = AHRS::Cov(10000*stationaryU); Matrix expected = (Matrix(3, 3) << 33.3333333, -1.66666667, 53.3333333, -1.66666667, 0.333333333, -5.16666667, @@ -54,7 +54,7 @@ TEST (AHRS, covU) { /* ************************************************************************* */ TEST (AHRS, covF) { - Matrix actual = cov(100*stationaryF); + Matrix actual = AHRS::Cov(100*stationaryF); Matrix expected = (Matrix(3, 3) << 63.3808333, -0.432166667, -48.1706667, -0.432166667, 8.31053333, -16.6792667, diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 6cfcd93e60..efe1df640d 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 238bece62c..b468a2b6e7 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 9aa87fb83e..ddb5cf7a23 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 0e5f6421f3..573352e877 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 05260521ec..4691da3842 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/testTOAFactor.cpp b/gtsam_unstable/slam/tests/testTOAFactor.cpp new file mode 100644 index 0000000000..879f7095e8 --- /dev/null +++ b/gtsam_unstable/slam/tests/testTOAFactor.cpp @@ -0,0 +1,133 @@ +/* ---------------------------------------------------------------------------- + + * 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 testTOAFactor.cpp + * @brief Unit tests for "Time of Arrival" factor + * @author Frank Dellaert + * @author Jay Chakravarty + * @date December 2014 + */ + +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +// typedefs +typedef Eigen::Matrix Vector1; +typedef Expression Double_; +typedef Expression Point3_; +typedef Expression Event_; + +// units +static const double ms = 1e-3; +static const double cm = 1e-2; + +// Create a noise model for the TOA error +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(1, 0.5 * ms)); + +static const double timeOfEvent = 25; +static const Event exampleEvent(timeOfEvent, 1, 0, 0); +static const Point3 microphoneAt0; + +//***************************************************************************** +TEST( TOAFactor, NewWay ) { + Key key = 12; + Event_ eventExpression(key); + Point3_ microphoneConstant(microphoneAt0); // constant expression + double measurement = 7; + Double_ expression(&Event::toa, eventExpression, microphoneConstant); + ExpressionFactor factor(model, measurement, expression); +} + +//***************************************************************************** +TEST( TOAFactor, WholeEnchilada ) { + + static const bool verbose = false; + + // Create microphones + const double height = 0.5; + vector microphones; + microphones.push_back(Point3(0, 0, height)); + microphones.push_back(Point3(403 * cm, 0, height)); + microphones.push_back(Point3(403 * cm, 403 * cm, height)); + microphones.push_back(Point3(0, 403 * cm, 2 * height)); + EXPECT_LONGS_EQUAL(4, microphones.size()); +// microphones.push_back(Point3(200 * cm, 200 * cm, height)); + + // Create a ground truth point + const double timeOfEvent = 0; + Event groundTruthEvent(timeOfEvent, 245 * cm, 201.5 * cm, (212 - 45) * cm); + + // Simulate simulatedTOA + size_t K = microphones.size(); + vector simulatedTOA(K); + for (size_t i = 0; i < K; i++) { + simulatedTOA[i] = groundTruthEvent.toa(microphones[i]); + if (verbose) { + cout << "mic" << i << " = " << microphones[i] << endl; + cout << "z" << i << " = " << simulatedTOA[i] / ms << endl; + } + } + + // Now, estimate using non-linear optimization + ExpressionFactorGraph graph; + Key key = 12; + Event_ eventExpression(key); + for (size_t i = 0; i < K; i++) { + Point3_ microphone_i(microphones[i]); // constant expression + Double_ predictTOA(&Event::toa, eventExpression, microphone_i); + graph.addExpressionFactor(predictTOA, simulatedTOA[i], model); + } + + /// Print the graph + if (verbose) + GTSAM_PRINT(graph); + + // Create initial estimate + Values initialEstimate; + //Event estimatedEvent(timeOfEvent -10, 200 * cm, 150 * cm, 350 * cm); + Vector4 delta; + delta << 0.1, 0.1, -0.1, 0.1; + Event estimatedEvent = groundTruthEvent.retract(delta); + initialEstimate.insert(key, estimatedEvent); + + // Print + if (verbose) + initialEstimate.print("Initial Estimate:\n"); + + // Optimize using Levenberg-Marquardt optimization. + LevenbergMarquardtParams params; + params.setAbsoluteErrorTol(1e-10); + if (verbose) + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, params); + Values result = optimizer.optimize(); + if (verbose) + result.print("Final Result:\n"); + + EXPECT(assert_equal(groundTruthEvent, result.at(key), 1e-6)); +} +//***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//***************************************************************************** + diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index a8a3ae5e9a..ebb52d2762 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 c4891a6150..349cdeea40 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/matlab/+gtsam/plot3DTrajectory.m b/matlab/+gtsam/plot3DTrajectory.m index 05483e5894..2900aed99c 100644 --- a/matlab/+gtsam/plot3DTrajectory.m +++ b/matlab/+gtsam/plot3DTrajectory.m @@ -33,12 +33,12 @@ function plot3DTrajectory(values,linespec,frames,scale,marginals) end gtsam.plotPose3(lastPose, P, scale); catch err - warning(['no Pose3 at ' lastKey]); + % warning(['no Pose3 at ' lastKey]); end - lastIndex = i; end + lastIndex = i; catch - warning(['no Pose3 at ' key]); + % warning(['no Pose3 at ' key]); end % Draw final pose @@ -53,7 +53,7 @@ function plot3DTrajectory(values,linespec,frames,scale,marginals) end gtsam.plotPose3(lastPose, P, scale); catch - warning(['no Pose3 at ' lastIndex]); + % warning(['no Pose3 at ' lastIndex]); end end diff --git a/matlab/gtsam_examples/IMUKittiExampleGPS.m b/matlab/gtsam_examples/IMUKittiExampleGPS.m index e205d918ce..32f61e47f0 100644 --- a/matlab/gtsam_examples/IMUKittiExampleGPS.m +++ b/matlab/gtsam_examples/IMUKittiExampleGPS.m @@ -33,7 +33,7 @@ %% Get initial conditions for the estimated trajectory currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame) -currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Precisions([ 0.0; 0.0; 0.0; 1; 1; 1 ]); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0); @@ -72,7 +72,7 @@ newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentBiasKey, currentBias); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); else t_previous = GPS_data(measurementIndex-1, 1).Time; diff --git a/matlab/gtsam_examples/MonocularVOExample.m b/matlab/gtsam_examples/MonocularVOExample.m index 9fd2df4631..11c4253dee 100644 --- a/matlab/gtsam_examples/MonocularVOExample.m +++ b/matlab/gtsam_examples/MonocularVOExample.m @@ -40,7 +40,7 @@ %% Create initial estimate initial = Values; -trueE = EssentialMatrix(aRb,Sphere2(aTb)); +trueE = EssentialMatrix(aRb,Unit3(aTb)); initialE = trueE.retract([0.1, -0.1, 0.1, 0.1, -0.1]'); initial.insert(1, initialE); @@ -52,5 +52,5 @@ %% Print result (as essentialMatrix) and error error = graph.error(result) -E = result.at(1) +E = result.atEssentialMatrix(1) diff --git a/matlab/gtsam_tests/testPriorFactor.m b/matlab/gtsam_tests/testPriorFactor.m new file mode 100644 index 0000000000..4d567a6ce8 --- /dev/null +++ b/matlab/gtsam_tests/testPriorFactor.m @@ -0,0 +1,18 @@ +% test wrapping of Values +import gtsam.*; + +values = Values; + +key = 5; +priorPose3 = Pose3; +model = noiseModel.Unit.Create(6); +factor = PriorFactorPose3(key, priorPose3, model); +values.insert(key, priorPose3); +EXPECT('error', factor.error(values) == 0); + +key = 3; +priorVector = [0,0,0]'; +model = noiseModel.Unit.Create(3); +factor = PriorFactorVector(key, priorVector, model); +values.insert(key, priorVector); +EXPECT('error', factor.error(values) == 0); diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index e08019610a..1a6856a9a6 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -1,5 +1,8 @@ % Test runner script - runs each test +% display 'Starting: testPriorFactor' +% testPriorFactor + display 'Starting: testValues' testValues diff --git a/matlab/unstable_examples/FlightCameraTransformIMU.m b/matlab/unstable_examples/FlightCameraTransformIMU.m index 1b05d28771..b0b37ee338 100644 --- a/matlab/unstable_examples/FlightCameraTransformIMU.m +++ b/matlab/unstable_examples/FlightCameraTransformIMU.m @@ -57,7 +57,7 @@ isam = ISAM2(isamParams); %% Get initial conditions for the estimated trajectory -currentVelocityGlobal = LieVector([10;0;0]); % (This is slightly wrong!) +currentVelocityGlobal = [10;0;0]; % (This is slightly wrong!) Zhaoyang: Fixed currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); @@ -101,7 +101,7 @@ for i=1:size(trajectory)-1 xKey = symbol('x',i); - pose = trajectory.at(xKey); % GT pose + pose = trajectory.atPose3(xKey); % GT pose pose_t = pose.translation(); % GT pose-translation if exist('h_cursor','var') @@ -165,13 +165,13 @@ if i > 1 xKey_prev = symbol('x',i-1); - pose_prev = trajectory.at(xKey_prev); + pose_prev = trajectory.atPose3(xKey_prev); step = pose_prev.between(pose); % insert estimate for current pose with some normal noise on % translation - initial.insert(xKey,result.at(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)]))); + initial.insert(xKey,result.atPose3(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)]))); % visual measurements if measurements.size > 0 && use_camera @@ -181,12 +181,12 @@ zKey = measurementKeys.at(zz); lKey = symbol('l',symbolIndex(zKey)); - fg.add(TransformCalProjectionFactorCal3_S2(measurements.at(zKey), ... + fg.add(TransformCalProjectionFactorCal3_S2(measurements.atPoint2(zKey), ... z_cov, xKey, transformKey, lKey, calibrationKey, false, true)); % only add landmark to values if doesn't exist yet if ~result.exists(lKey) - noisy_landmark = landmarks.at(lKey).compose(Point3(normrnd(0,landmark_noise,3,1))); + noisy_landmark = landmarks.atPoint3(lKey).compose(Point3(normrnd(0,landmark_noise,3,1))); initial.insert(lKey, noisy_landmark); % and add a prior since its position is known diff --git a/matlab/unstable_examples/IMUKittiExampleAdvanced.m b/matlab/unstable_examples/IMUKittiExampleAdvanced.m index 1db60a5ad6..cb13eacee2 100644 --- a/matlab/unstable_examples/IMUKittiExampleAdvanced.m +++ b/matlab/unstable_examples/IMUKittiExampleAdvanced.m @@ -7,7 +7,7 @@ %% Read metadata and compute relative sensor pose transforms % IMU metadata disp('-- Reading sensor metadata') -IMU_metadata = importdata('KittiEquivBiasedImu_metadata.txt'); +IMU_metadata = importdata(findExampleDataFile('KittiEquivBiasedImu_metadata.txt')); IMU_metadata = cell2struct(num2cell(IMU_metadata.data), IMU_metadata.colheaders, 2); IMUinBody = Pose3.Expmap([IMU_metadata.BodyPtx; IMU_metadata.BodyPty; IMU_metadata.BodyPtz; IMU_metadata.BodyPrx; IMU_metadata.BodyPry; IMU_metadata.BodyPrz; ]); @@ -16,14 +16,14 @@ end % VO metadata -VO_metadata = importdata('KittiRelativePose_metadata.txt'); +VO_metadata = importdata(findExampleDataFile('KittiRelativePose_metadata.txt')); VO_metadata = cell2struct(num2cell(VO_metadata.data), VO_metadata.colheaders, 2); VOinBody = Pose3.Expmap([VO_metadata.BodyPtx; VO_metadata.BodyPty; VO_metadata.BodyPtz; VO_metadata.BodyPrx; VO_metadata.BodyPry; VO_metadata.BodyPrz; ]); VOinIMU = IMUinBody.inverse().compose(VOinBody); % GPS metadata -GPS_metadata = importdata('KittiGps_metadata.txt'); +GPS_metadata = importdata(findExampleDataFile('KittiGps_metadata.txt')); GPS_metadata = cell2struct(num2cell(GPS_metadata.data), GPS_metadata.colheaders, 2); GPSinBody = Pose3.Expmap([GPS_metadata.BodyPtx; GPS_metadata.BodyPty; GPS_metadata.BodyPtz; GPS_metadata.BodyPrx; GPS_metadata.BodyPry; GPS_metadata.BodyPrz; ]); @@ -32,7 +32,7 @@ %% Read data and change coordinate frame of GPS and VO measurements to IMU frame disp('-- Reading sensor data from file') % IMU data -IMU_data = importdata('KittiEquivBiasedImu.txt'); +IMU_data = importdata(findExampleDataFile('KittiEquivBiasedImu.txt')); IMU_data = cell2struct(num2cell(IMU_data.data), IMU_data.colheaders, 2); imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_data.accelZ]' [IMU_data.omegaX]' [IMU_data.omegaY]' [IMU_data.omegaZ]' ], 2), 'UniformOutput', false); [IMU_data.acc_omega] = deal(imum{:}); @@ -40,7 +40,7 @@ clear imum % VO data -VO_data = importdata('KittiRelativePose.txt'); +VO_data = importdata(findExampleDataFile('KittiRelativePose.txt')); VO_data = cell2struct(num2cell(VO_data.data), VO_data.colheaders, 2); % Merge relative pose fields and convert to Pose3 logposes = [ [VO_data.dtx]' [VO_data.dty]' [VO_data.dtz]' [VO_data.drx]' [VO_data.dry]' [VO_data.drz]' ]; @@ -71,7 +71,7 @@ %% Get initial conditions for the estimated trajectory % % % currentPoseGlobal = Pose3(Rot3, GPS_data(firstGPSPose).Position); % initial pose is the reference frame (navigation frame) currentPoseGlobal = Pose3; -currentVelocityGlobal = LieVector([0;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_x = noiseModel.Isotropic.Sigma(6, 0.01); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1000.0); @@ -120,7 +120,7 @@ newValues.insert(currentVelKey, currentVelocityGlobal); newValues.insert(currentBiasKey, currentBias); newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseGlobal, sigma_init_x)); - newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + newFactors.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); newFactors.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); else t_previous = timestamps(measurementIndex-1, 1); diff --git a/matlab/unstable_examples/SmartRangeFactorExample.m b/matlab/unstable_examples/SmartRangeFactorExample.m index 93e5dce0cc..7535447dfb 100644 --- a/matlab/unstable_examples/SmartRangeFactorExample.m +++ b/matlab/unstable_examples/SmartRangeFactorExample.m @@ -82,8 +82,8 @@ key_prev = poseKey(ind_pose-1); key_curr = poseKey(ind_pose); - prev_pose = smartValues.at(key_prev); - curr_pose = smartValues.at(key_curr); + prev_pose = smartValues.atPose2(key_prev); + curr_pose = smartValues.atPose2(key_curr); GTDeltaPose = prev_pose.between(curr_pose); noisePose = Pose2([t_noise*rand; t_noise*rand; r_noise*rand]); NoisyDeltaPose = GTDeltaPose.compose(noisePose); diff --git a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m index 236327b052..c9e912ea45 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorExampleISAM.m @@ -96,17 +96,17 @@ if i > 1 if i < 11 - initial.insert(i,result.at(i-1).compose(move_forward)); + initial.insert(i,result.atPose3(i-1).compose(move_forward)); fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); else - initial.insert(i,result.at(i-1).compose(move_circle)); + initial.insert(i,result.atPose3(i-1).compose(move_circle)); fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); end end % generate some camera measurements - cam_pose = initial.at(i).compose(actual_transform); + cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); cam = SimpleCamera(cam_pose,K); i @@ -137,10 +137,10 @@ hold on; %% plot results - result_camera_transform = result.at(transformKey); + result_camera_transform = result.atPose3(transformKey); for j=1:i - gtsam.plotPose3(result.at(j),[],0.5); - gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5); + gtsam.plotPose3(result.atPose3(j),[],0.5); + gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5); end xlabel('x (m)'); @@ -153,12 +153,12 @@ % axis equal for l=101:100+nrPoints - plotPoint3(result.at(l),'g'); + plotPoint3(result.atPoint3(l),'g'); end - ty = result.at(transformKey).translation().y(); - fx = result.at(calibrationKey).fx(); - fy = result.at(calibrationKey).fy(); + ty = result.atPose3(transformKey).translation().y(); + fx = result.atCal3_S2(calibrationKey).fx(); + fy = result.atCal3_S2(calibrationKey).fy(); text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty)); text(1,5,3,sprintf('fx(900): %.0f',fx)); text(1,5,1,sprintf('fy(900): %.0f',fy)); @@ -180,10 +180,10 @@ fprintf('Cheirality Exception count: %d\n', cheirality_exception_count); disp('Transform after optimization'); -result.at(transformKey) +result.atPose3(transformKey) disp('Calibration after optimization'); -result.at(calibrationKey) +result.atCal3_S2(calibrationKey) diff --git a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m index 834472a7d5..fd4a174693 100644 --- a/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m +++ b/matlab/unstable_examples/TransformCalProjectionFactorIMUExampleISAM.m @@ -89,7 +89,7 @@ currentIMUPoseGlobal = Pose3(); %% Get initial conditions for the estimated trajectory -currentVelocityGlobal = LieVector([1;0;0]); % the vehicle is stationary at the beginning +currentVelocityGlobal = [1;0;0]; % the vehicle is stationary at the beginning currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0); @@ -127,7 +127,7 @@ fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov)); % velocity and bias evolution - fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); + fg.add(PriorFactorVector(currentVelKey, currentVelocityGlobal, sigma_init_v)); fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b)); result = initial; diff --git a/matlab/unstable_examples/TransformProjectionFactorExample.m b/matlab/unstable_examples/TransformProjectionFactorExample.m index 410b7df0fc..669073e569 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExample.m +++ b/matlab/unstable_examples/TransformProjectionFactorExample.m @@ -61,17 +61,17 @@ for i=1:20 if i > 1 if i < 11 - initial.insert(i,initial.at(i-1).compose(move_forward)); + initial.insert(i,initial.atPose3(i-1).compose(move_forward)); fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); else - initial.insert(i,initial.at(i-1).compose(move_circle)); + initial.insert(i,initial.atPose3(i-1).compose(move_circle)); fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); end end % generate some camera measurements - cam_pose = initial.at(i).compose(actual_transform); + cam_pose = initial.atPose3(i).compose(actual_transform); gtsam.plotPose3(cam_pose); cam = SimpleCamera(cam_pose,K); i @@ -93,14 +93,14 @@ %% camera plotting for i=1:20 - gtsam.plotPose3(initial.at(i).compose(camera_transform)); + gtsam.plotPose3(initial.atPose3(i).compose(camera_transform)); end xlabel('x (m)'); ylabel('y (m)'); disp('Transform before optimization'); -initial.at(1000) +initial.atPose3(1000) params = LevenbergMarquardtParams; params.setAbsoluteErrorTol(1e-15); @@ -112,7 +112,7 @@ result = optimizer.optimizeSafely(); disp('Transform after optimization'); -result.at(1000) +result.atPose3(1000) % results is empty here. optimizer doesn't generate result? axis([0 25 0 25 0 10]); axis equal diff --git a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m index 169736f4b0..8edcfade7f 100644 --- a/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m +++ b/matlab/unstable_examples/TransformProjectionFactorExampleISAM.m @@ -86,17 +86,17 @@ if i > 1 if i < 11 - initial.insert(i,result.at(i-1).compose(move_forward)); + initial.insert(i,result.atPose3(i-1).compose(move_forward)); fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance)); else - initial.insert(i,result.at(i-1).compose(move_circle)); + initial.insert(i,result.atPose3(i-1).compose(move_circle)); fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance)); end end % generate some camera measurements - cam_pose = initial.at(i).compose(actual_transform); + cam_pose = initial.atPose3(i).compose(actual_transform); % gtsam.plotPose3(cam_pose); cam = SimpleCamera(cam_pose,K); i @@ -127,10 +127,10 @@ hold on; %% plot results - result_camera_transform = result.at(1000); + result_camera_transform = result.atPose3(1000); for j=1:i - gtsam.plotPose3(result.at(j)); - gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5); + gtsam.plotPose3(result.atPose3(j)); + gtsam.plotPose3(result.atPose3(j).compose(result_camera_transform),[],0.5); end xlabel('x (m)'); @@ -143,10 +143,10 @@ % axis equal for l=101:100+nrPoints - plotPoint3(result.at(l),'g'); + plotPoint3(result.atPoint3(l),'g'); end - ty = result.at(1000).translation().y(); + ty = result.atPose3(1000).translation().y(); text(5,5,5,sprintf('Y-Transform: %0.2g',ty)); if(write_video) @@ -168,7 +168,7 @@ disp('Transform after optimization'); -result.at(1000) +result.atPose3(1000) diff --git a/matlab/unstable_examples/plot_projected_landmarks.m b/matlab/unstable_examples/plot_projected_landmarks.m index cfe08ec541..6b8101844e 100644 --- a/matlab/unstable_examples/plot_projected_landmarks.m +++ b/matlab/unstable_examples/plot_projected_landmarks.m @@ -23,7 +23,7 @@ for i = 0:measurement_keys.size-1 key = measurement_keys.at(i); key_index = gtsam.symbolIndex(key); - p = landmarks.at(gtsam.symbol('l',key_index)); + p = landmarks.atPoint3(gtsam.symbol('l',key_index)); x(i+1) = p.x; y(i+1) = p.y; diff --git a/matlab/unstable_examples/project_landmarks.m b/matlab/unstable_examples/project_landmarks.m index b11c43bc55..629c6d9946 100644 --- a/matlab/unstable_examples/project_landmarks.m +++ b/matlab/unstable_examples/project_landmarks.m @@ -8,7 +8,7 @@ measurements = Values; for i=1:size(landmarks)-1 - z = camera.project(landmarks.at(symbol('l',i))); + z = camera.project(landmarks.atPoint3(symbol('l',i))); % check bounding box if z.x < 0 || z.x > 1280 diff --git a/matlab/unstable_examples/testTSAMFactors.m b/matlab/unstable_examples/testTSAMFactors.m index abdfc59224..341725723f 100644 --- a/matlab/unstable_examples/testTSAMFactors.m +++ b/matlab/unstable_examples/testTSAMFactors.m @@ -48,12 +48,12 @@ result = optimizer.optimize(); % Check result -CHECK('error',result.at(100).equals(b1,1e-5)) -CHECK('error',result.at(10).equals(origin,1e-5)) -CHECK('error',result.at(1).equals(Point2(0,1),1e-5)) -CHECK('error',result.at(2).equals(Point2(0,1),1e-5)) -CHECK('error',result.at(20).equals(origin,1e-5)) -CHECK('error',result.at(200).equals(b2,1e-5)) +CHECK('error',result.atPose2(100).equals(b1,1e-5)) +CHECK('error',result.atPose2(10).equals(origin,1e-5)) +CHECK('error',result.atPoint2(1).equals(Point2(0,1),1e-5)) +CHECK('error',result.atPoint2(2).equals(Point2(0,1),1e-5)) +CHECK('error',result.atPose2(20).equals(origin,1e-5)) +CHECK('error',result.atPose2(200).equals(b2,1e-5)) % Check error CHECK('error',abs(graph.error(result))<1e-9) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 008dbb78d6..068b39ecac 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -2,7 +2,7 @@ # note the source dir on each set (tests_exclude "") -if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") # might not be best test - Richard & Jason & Frank +if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richard & Jason & Frank # clang linker segfaults on large testSerializationSLAM list (APPEND tests_exclude "testSerializationSLAM.cpp") endif() diff --git a/tests/simulated2D.h b/tests/simulated2D.h index b67ef0aef8..37d2455eb8 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 6ddf9ebdbe..f593ab23a8 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 b7cb7aefe9..9866d22aa2 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 8748e74643..a7bcf7153e 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 @@ class NonlinearMotionModel : public NoiseModelFactor2 { 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 @@ class NonlinearMotionModel : public NoiseModelFactor2 { } // 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 @@ class NonlinearMotionModel : public NoiseModelFactor2 { /** 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 @@ class NonlinearMotionModel : public NoiseModelFactor2 { } /** 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 @@ class NonlinearMeasurementModel : public NoiseModelFactor1 { // 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 @@ class NonlinearMeasurementModel : public NoiseModelFactor1 { 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 @@ class NonlinearMeasurementModel : public NoiseModelFactor1 { } // 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 @@ class NonlinearMeasurementModel : public NoiseModelFactor1 { /** 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 @@ class NonlinearMeasurementModel : public NoiseModelFactor1 { } /** 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 1927ba5c6a..f6180fb738 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))); -} - -/* ************************************************************************* */ -// 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)); + //DefaultChart chart6; + EXPECT(assert_equal(zero(3), traits::Local(R, R))); } -/* ************************************************************************* */ +//****************************************************************************** int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//****************************************************************************** diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 96039a3ccd..a3c99ece79 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 ad2c0767a1..89c3d5cf87 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(); - - /* prepare the buffer of block diagonals */ - std::vector blocks; blocks.reserve(n); - - /* 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; - } - - /* 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."); - } - } - - return blocks; -} +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"); + + // 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"); + + // Solve the system using direct method + VectorValues deltaDirect = simpleGFG.optimize(); + EXPECT(assert_equal(expectedSolution, deltaDirect, 1e-5)); + //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(expectedSolution, deltaPCGDummy, 1e-5)); + //deltaPCGDummy.print("PCG Dummy"); + + // With Block-Jacobi preconditioner + pcg->preconditioner_ = boost::make_shared(); + VectorValues deltaPCGJacobi = PCGSolver(*pcg).optimize(simpleGFG); + EXPECT(assert_equal(expectedSolution, deltaPCGJacobi, 1e-5)); + //deltaPCGJacobi.print("PCG Jacobi"); -/* ************************************************************************* */ -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)); - - // 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 ffde900e95..2514c80d9a 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 95% rename from gtsam_unstable/timing/timeAdaptAutoDiff.cpp rename to timing/timeAdaptAutoDiff.cpp index be11453b5f..edfd420efd 100644 --- a/gtsam_unstable/timing/timeAdaptAutoDiff.cpp +++ b/timing/timeAdaptAutoDiff.cpp @@ -18,8 +18,8 @@ #include "timeLinearize.h" #include -#include -#include +#include +#include #include #include #include diff --git a/gtsam_unstable/timing/timeCameraExpression.cpp b/timing/timeCameraExpression.cpp similarity index 94% rename from gtsam_unstable/timing/timeCameraExpression.cpp rename to timing/timeCameraExpression.cpp index 92522c4409..208c991fd2 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, - boost::optional H1, boost::optional 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 6f2909b7a4..d82ef94429 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 4cb6558253..962e7ee216 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 06ab633c64..92aece7e5d 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 6860914ef3..4ad9d7d478 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 678e4db601..cfb8e0dc6c 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 d15ccfaf43..c4196f4142 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)); diff --git a/wrap/Argument.cpp b/wrap/Argument.cpp index 1f57917d88..848998eb0d 100644 --- a/wrap/Argument.cpp +++ b/wrap/Argument.cpp @@ -78,11 +78,11 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { string cppType = type.qualifiedName("::"); string matlabUniqueType = type.qualifiedName(); - if (is_ptr) + if (is_ptr && type.category != Qualified::EIGEN) // A pointer: emit an "unwrap_shared_ptr" call which returns a pointer file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< "; - else if (is_ref) + else if (is_ref && type.category != Qualified::EIGEN) // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; else @@ -94,7 +94,7 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const { file.oss << cppType << " " << name << " = unwrap< "; file.oss << cppType << " >(" << matlabName; - if (is_ptr || is_ref) + if( (is_ptr || is_ref) && type.category != Qualified::EIGEN) file.oss << ", \"ptr_" << matlabUniqueType << "\""; file.oss << ");" << endl; } diff --git a/wrap/Module.cpp b/wrap/Module.cpp index 55fd13715a..092c732f78 100644 --- a/wrap/Module.cpp +++ b/wrap/Module.cpp @@ -156,6 +156,7 @@ void Module::parseMarkup(const std::string& data) { >> (*(basic.namespace_p >> str_p("::")) >> basic.className_p)[assign_a(fwDec.name)] >> ch_p(';') [push_back_a(forward_declarations, fwDec)] + [assign_a(cls,cls0)] // also clear class to avoid partial parse [assign_a(fwDec, fwDec0)]; Rule module_content_p = basic.comments_p | include_p | class_p diff --git a/wrap/matlab.h b/wrap/matlab.h index 23f3919035..1639876ccc 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -425,3 +425,20 @@ boost::shared_ptr unwrap_shared_ptr(const mxArray* obj, const string& pro boost::shared_ptr* spp = *reinterpret_cast**> (mxGetData(mxh)); return *spp; } + +//// throw an error if unwrap_shared_ptr is attempted for an Eigen Vector +//template <> +//Vector unwrap_shared_ptr(const mxArray* obj, const string& propertyName) { +// bool unwrap_shared_ptr_Vector_attempted = false; +// BOOST_STATIC_ASSERT(unwrap_shared_ptr_Vector_attempted, "Vector cannot be unwrapped as a shared pointer"); +// return Vector(); +//} + +//// throw an error if unwrap_shared_ptr is attempted for an Eigen Matrix +//template <> +//Matrix unwrap_shared_ptr(const mxArray* obj, const string& propertyName) { +// bool unwrap_shared_ptr_Matrix_attempted = false; +// BOOST_STATIC_ASSERT(unwrap_shared_ptr_Matrix_attempted, "Matrix cannot be unwrapped as a shared pointer"); +// return Matrix(); +//} + diff --git a/wrap/tests/expected/geometry_wrapper.cpp b/wrap/tests/expected/geometry_wrapper.cpp index 527600b474..585918f20b 100644 --- a/wrap/tests/expected/geometry_wrapper.cpp +++ b/wrap/tests/expected/geometry_wrapper.cpp @@ -363,7 +363,7 @@ void Test_arg_EigenConstRef_24(int nargout, mxArray *out[], int nargin, const mx typedef boost::shared_ptr Shared; checkArguments("arg_EigenConstRef",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->arg_EigenConstRef(value); } @@ -707,7 +707,7 @@ void MyTemplatePoint2_templatedMethod_57(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } @@ -736,7 +736,7 @@ void MyTemplatePoint2_templatedMethod_60(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector"); + Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } @@ -795,7 +795,7 @@ void MyTemplateMatrix_accept_T_65(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->accept_T(value); } @@ -804,7 +804,7 @@ void MyTemplateMatrix_accept_Tptr_66(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->accept_Tptr(value); } @@ -842,7 +842,7 @@ void MyTemplateMatrix_return_T_69(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_T(value)); } @@ -851,7 +851,7 @@ void MyTemplateMatrix_return_Tptr_70(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); { SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value)); out[0] = wrap_shared_ptr(ret,"Matrix"); @@ -863,8 +863,8 @@ void MyTemplateMatrix_return_ptrs_71(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr p1 = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); - boost::shared_ptr p2 = unwrap_shared_ptr< Matrix >(in[2], "ptr_Matrix"); + Matrix p1 = unwrap< Matrix >(in[1]); + Matrix p2 = unwrap< Matrix >(in[2]); pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2); { SharedMatrix* ret = new SharedMatrix(pairResult.first); @@ -881,7 +881,7 @@ void MyTemplateMatrix_templatedMethod_72(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } @@ -910,7 +910,7 @@ void MyTemplateMatrix_templatedMethod_75(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector"); + Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } diff --git a/wrap/tests/expected2/geometry_wrapper.cpp b/wrap/tests/expected2/geometry_wrapper.cpp index 82926e2ce6..4b41f3958a 100644 --- a/wrap/tests/expected2/geometry_wrapper.cpp +++ b/wrap/tests/expected2/geometry_wrapper.cpp @@ -335,7 +335,7 @@ void Test_arg_EigenConstRef_22(int nargout, mxArray *out[], int nargin, const mx typedef boost::shared_ptr Shared; checkArguments("arg_EigenConstRef",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_Test"); - Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->arg_EigenConstRef(value); } @@ -679,7 +679,7 @@ void MyTemplatePoint2_templatedMethod_55(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } @@ -708,7 +708,7 @@ void MyTemplatePoint2_templatedMethod_58(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplatePoint2"); - Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector"); + Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } @@ -767,7 +767,7 @@ void MyTemplateMatrix_accept_T_63(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("accept_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Matrix& value = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->accept_T(value); } @@ -776,7 +776,7 @@ void MyTemplateMatrix_accept_Tptr_64(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("accept_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); obj->accept_Tptr(value); } @@ -814,7 +814,7 @@ void MyTemplateMatrix_return_T_67(int nargout, mxArray *out[], int nargin, const typedef boost::shared_ptr Shared; checkArguments("return_T",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->return_T(value)); } @@ -823,7 +823,7 @@ void MyTemplateMatrix_return_Tptr_68(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("return_Tptr",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr value = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix value = unwrap< Matrix >(in[1]); { SharedMatrix* ret = new SharedMatrix(obj->return_Tptr(value)); out[0] = wrap_shared_ptr(ret,"Matrix"); @@ -835,8 +835,8 @@ void MyTemplateMatrix_return_ptrs_69(int nargout, mxArray *out[], int nargin, co typedef boost::shared_ptr Shared; checkArguments("return_ptrs",nargout,nargin-1,2); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - boost::shared_ptr p1 = unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); - boost::shared_ptr p2 = unwrap_shared_ptr< Matrix >(in[2], "ptr_Matrix"); + Matrix p1 = unwrap< Matrix >(in[1]); + Matrix p2 = unwrap< Matrix >(in[2]); pair< SharedMatrix, SharedMatrix > pairResult = obj->return_ptrs(p1,p2); { SharedMatrix* ret = new SharedMatrix(pairResult.first); @@ -853,7 +853,7 @@ void MyTemplateMatrix_templatedMethod_70(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodMatrix",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Matrix& t = *unwrap_shared_ptr< Matrix >(in[1], "ptr_Matrix"); + Matrix t = unwrap< Matrix >(in[1]); out[0] = wrap< Matrix >(obj->templatedMethod(t)); } @@ -882,7 +882,7 @@ void MyTemplateMatrix_templatedMethod_73(int nargout, mxArray *out[], int nargin typedef boost::shared_ptr Shared; checkArguments("templatedMethodVector",nargout,nargin-1,1); Shared obj = unwrap_shared_ptr(in[0], "ptr_MyTemplateMatrix"); - Vector& t = *unwrap_shared_ptr< Vector >(in[1], "ptr_Vector"); + Vector t = unwrap< Vector >(in[1]); out[0] = wrap< Vector >(obj->templatedMethod(t)); } diff --git a/wrap/tests/geometry.h b/wrap/tests/geometry.h index 78e2a1dff2..69bc7e3be7 100644 --- a/wrap/tests/geometry.h +++ b/wrap/tests/geometry.h @@ -1,7 +1,7 @@ // comments! class VectorNotEigen; -class ns::OtherClass; +virtual class ns::OtherClass; namespace gtsam { diff --git a/wrap/tests/testClass.cpp b/wrap/tests/testClass.cpp index 756b6668dd..a133e15aca 100644 --- a/wrap/tests/testClass.cpp +++ b/wrap/tests/testClass.cpp @@ -160,9 +160,103 @@ TEST( Class, Grammar ) { EXPECT_LONGS_EQUAL(ReturnType::EIGEN, rv4.type1.category); } -/* ************************************************************************* */ +//****************************************************************************** +TEST( Class, TemplateSubstition ) { + + using classic::space_p; + + // Create type grammar that will place result in cls + Class cls; + Template t; + ClassGrammar g(cls, t); + + string markup(string("template" + "class Point2 {" + " T myMethod(const T& t) const;" + "};")); + + EXPECT(parse(markup.c_str(), g, space_p).full); + + // Method 2 + Method m2 = cls.method("myMethod"); + EXPECT(assert_equal("myMethod", m2.name())); + EXPECT(m2.isConst()); + LONGS_EQUAL(1, m2.nrOverloads()); + + ReturnValue rv2 = m2.returnValue(0); + EXPECT(!rv2.isPair); + EXPECT(!rv2.type1.isPtr); + EXPECT(assert_equal("T", rv2.type1.name())); + EXPECT_LONGS_EQUAL(ReturnType::CLASS, rv2.type1.category); + + // Generate some expected values for qualified types + Qualified q_void("void", Qualified::VOID); + Qualified q_double("double", Qualified::BASIS); + Qualified q_Matrix("Matrix", Qualified::EIGEN); + Qualified q_Point3("Point3", Qualified::CLASS); + + EXPECT_LONGS_EQUAL(4, t.nrValues()); + EXPECT(t.argName()=="T"); + EXPECT(t[0]==q_void); + EXPECT(t[1]==q_double); + EXPECT(t[2]==q_Matrix); + EXPECT(t[3]==q_Point3); + + vector classes = cls.expandTemplate(t.argName(), t.argValues()); + + // check the number of new classes is four + EXPECT_LONGS_EQUAL(4, classes.size()); + + // check return types + EXPECT( classes[0].method("myMethod").returnValue(0).type1 == q_void); + EXPECT( classes[1].method("myMethod").returnValue(0).type1 == q_double); + EXPECT( classes[2].method("myMethod").returnValue(0).type1 == q_Matrix); + EXPECT( classes[3].method("myMethod").returnValue(0).type1 == q_Point3); + + // check the argument types + EXPECT( classes[0].method("myMethod").argumentList(0)[0].type == q_void); + EXPECT( classes[1].method("myMethod").argumentList(0)[0].type == q_double); + EXPECT( classes[2].method("myMethod").argumentList(0)[0].type == q_Matrix); + EXPECT( classes[3].method("myMethod").argumentList(0)[0].type == q_Point3); + +} + +//****************************************************************************** +TEST(Class, Template) { + + using classic::space_p; + + // Create type grammar that will place result in cls + Class cls; + Template t; + ClassGrammar g(cls, t); + + string markup( + string( + "template" + " virtual class PriorFactor : gtsam::NoiseModelFactor {" + " PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); " + " T prior() const; " + " void serialize() const; " + "};")); + + EXPECT(parse(markup.c_str(), g, space_p).full); + +} + +//****************************************************************************** +TEST( Class, Virtualness ) { + using classic::space_p; + Class cls; + Template t; + ClassGrammar g(cls, t); + string markup("virtual class Point3 {};"); + EXPECT(parse(markup.c_str(), g, space_p).full); + EXPECT(cls.isVirtual); +} +//****************************************************************************** int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//******************************************************************************