diff --git a/doc/Code/LocalizationExample2.cpp b/doc/Code/LocalizationExample2.cpp index d22180314..df9469a64 100644 --- a/doc/Code/LocalizationExample2.cpp +++ b/doc/Code/LocalizationExample2.cpp @@ -1,7 +1,7 @@ // add unary measurement factors, like GPS, on all three poses -noiseModel::Diagonal::shared_ptr unaryNoise = +auto unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y -graph.add(boost::make_shared(1, 0.0, 0.0, unaryNoise)); -graph.add(boost::make_shared(2, 2.0, 0.0, unaryNoise)); -graph.add(boost::make_shared(3, 4.0, 0.0, unaryNoise)); +graph.emplace_shared(1, 0.0, 0.0, unaryNoise); +graph.emplace_shared(2, 2.0, 0.0, unaryNoise); +graph.emplace_shared(3, 4.0, 0.0, unaryNoise); diff --git a/doc/Code/LocalizationFactor.cpp b/doc/Code/LocalizationFactor.cpp index d298091dc..2c1f01c43 100644 --- a/doc/Code/LocalizationFactor.cpp +++ b/doc/Code/LocalizationFactor.cpp @@ -1,13 +1,12 @@ class UnaryFactor: public NoiseModelFactor1 { double mx_, my_; ///< X and Y measurements - + public: UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model): NoiseModelFactor1(model, j), mx_(x), my_(y) {} - Vector evaluateError(const Pose2& q, - boost::optional H = boost::none) const - { + Vector evaluateError(const Pose2& q, + boost::optional H = boost::none) const override { const Rot2& R = q.rotation(); if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, diff --git a/doc/Code/OdometryExample.cpp b/doc/Code/OdometryExample.cpp index 2befa9dc2..7af27c60c 100644 --- a/doc/Code/OdometryExample.cpp +++ b/doc/Code/OdometryExample.cpp @@ -3,13 +3,11 @@ NonlinearFactorGraph graph; // Add a Gaussian prior on pose x_1 Pose2 priorMean(0.0, 0.0, 0.0); -noiseModel::Diagonal::shared_ptr priorNoise = - noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); -graph.addPrior(1, priorMean, priorNoise); +auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); +graph.add(PriorFactor(1, priorMean, priorNoise)); // Add two odometry factors Pose2 odometry(2.0, 0.0, 0.0); -noiseModel::Diagonal::shared_ptr odometryNoise = - noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); +auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.add(BetweenFactor(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor(2, 3, odometry, odometryNoise)); diff --git a/doc/Code/OdometryOutput1.txt b/doc/Code/OdometryOutput1.txt index cc34e8ef2..70aba38ee 100644 --- a/doc/Code/OdometryOutput1.txt +++ b/doc/Code/OdometryOutput1.txt @@ -1,11 +1,14 @@ Factor Graph: size: 3 -factor 0: PriorFactor on 1 - prior mean: (0, 0, 0) + +Factor 0: PriorFactor on 1 + prior mean: (0, 0, 0) noise model: diagonal sigmas [0.3; 0.3; 0.1]; -factor 1: BetweenFactor(1,2) - measured: (2, 0, 0) - noise model: diagonal sigmas [0.2; 0.2; 0.1]; -factor 2: BetweenFactor(2,3) - measured: (2, 0, 0) + +Factor 1: BetweenFactor(1,2) + measured: (2, 0, 0) noise model: diagonal sigmas [0.2; 0.2; 0.1]; + +Factor 2: BetweenFactor(2,3) + measured: (2, 0, 0) + noise model: diagonal sigmas [0.2; 0.2; 0.1]; \ No newline at end of file diff --git a/doc/Code/OdometryOutput2.txt b/doc/Code/OdometryOutput2.txt index acfa0b95d..6567bea6c 100644 --- a/doc/Code/OdometryOutput2.txt +++ b/doc/Code/OdometryOutput2.txt @@ -1,11 +1,23 @@ Initial Estimate: + Values with 3 values: -Value 1: (0.5, 0, 0.2) -Value 2: (2.3, 0.1, -0.2) -Value 3: (4.1, 0.1, 0.1) +Value 1: (gtsam::Pose2) +(0.5, 0, 0.2) + +Value 2: (gtsam::Pose2) +(2.3, 0.1, -0.2) + +Value 3: (gtsam::Pose2) +(4.1, 0.1, 0.1) Final Result: + Values with 3 values: -Value 1: (-1.8e-16, 8.7e-18, -9.1e-19) -Value 2: (2, 7.4e-18, -2.5e-18) -Value 3: (4, -1.8e-18, -3.1e-18) +Value 1: (gtsam::Pose2) +(7.5-16, -5.3-16, -1.8-16) + +Value 2: (gtsam::Pose2) +(2, -1.1-15, -2.5-16) + +Value 3: (gtsam::Pose2) +(4, -1.7-15, -2.5-16) diff --git a/doc/Code/OdometryOutput3.txt b/doc/Code/OdometryOutput3.txt index e346ccb4d..514d804cd 100644 --- a/doc/Code/OdometryOutput3.txt +++ b/doc/Code/OdometryOutput3.txt @@ -1,12 +1,12 @@ x1 covariance: - 0.09 1.1e-47 5.7e-33 - 1.1e-47 0.09 1.9e-17 - 5.7e-33 1.9e-17 0.01 + 0.09 1.7e-33 2.8e-33 +1.7e-33 0.09 2.6e-17 +2.8e-33 2.6e-17 0.01 x2 covariance: - 0.13 4.7e-18 2.4e-18 - 4.7e-18 0.17 0.02 - 2.4e-18 0.02 0.02 + 0.13 1.2e-18 6.1e-19 +1.2e-18 0.17 0.02 +6.1e-19 0.02 0.02 x3 covariance: - 0.17 2.7e-17 8.4e-18 - 2.7e-17 0.37 0.06 - 8.4e-18 0.06 0.03 + 0.17 8.6e-18 2.7e-18 +8.6e-18 0.37 0.06 +2.7e-18 0.06 0.03 \ No newline at end of file diff --git a/doc/gtsam.lyx b/doc/gtsam.lyx index 29d03cd35..482aaac25 100644 --- a/doc/gtsam.lyx +++ b/doc/gtsam.lyx @@ -134,14 +134,10 @@ A Hands-on Introduction \begin_layout Author Frank Dellaert -\begin_inset Newline newline -\end_inset - -Technical Report number GT-RIM-CP&R-2014-XXX \end_layout \begin_layout Date -September 2014 +Updated Last March 2022 \end_layout \begin_layout Standard @@ -162,7 +158,7 @@ Overview In this document I provide a hands-on introduction to both factor graphs and GTSAM. This is an updated version from the 2012 TR that is tailored to our GTSAM - 3.0 library and beyond. + 4.0 library and beyond. \end_layout \begin_layout Standard @@ -221,8 +217,10 @@ Georgia Tech Smoothing and Mapping It provides state of the art solutions to the SLAM and SFM problems, but can also be used to model and solve both simpler and more complex estimation problems. - It also provides a MATLAB interface which allows for rapid prototype developmen -t, visualization, and user interaction. + It also provides MATLAB and Python wrappers which allow for rapid prototype + development, visualization, and user interaction. + In addition, it is easy to use in Jupyter notebooks and/or Google's coLaborator +y. \end_layout \begin_layout Standard @@ -237,13 +235,13 @@ l complexity. \end_layout \begin_layout Standard -You can download the latest version of GTSAM at +You can download the latest version of GTSAM from GitHub at \begin_inset Flex URL status open \begin_layout Plain Layout -http://tinyurl.com/gtsam +https://github.com/borglab/gtsam \end_layout \end_inset @@ -741,7 +739,7 @@ noindent \begin_inset Formula $f_{0}(x_{1})$ \end_inset - on lines 5-8 as an instance of + on lines 5-7 as an instance of \series bold \emph on PriorFactor @@ -773,7 +771,7 @@ Pose2, noiseModel::Diagonal \series default \emph default - by specifying three standard deviations in line 7, respectively 30 cm. + by specifying three standard deviations in line 6, respectively 30 cm. \begin_inset space ~ \end_inset @@ -795,7 +793,7 @@ Similarly, odometry measurements are specified as Pose2 \series default \emph default - on line 11, with a slightly different noise model defined on line 12-13. + on line 10, with a slightly different noise model defined on line 11. We then add the two factors \begin_inset Formula $f_{1}(x_{1},x_{2};o_{1})$ \end_inset @@ -804,7 +802,7 @@ Pose2 \begin_inset Formula $f_{2}(x_{2},x_{3};o_{2})$ \end_inset - on lines 14-15, as instances of yet another templated class, + on lines 12-13, as instances of yet another templated class, \series bold \emph on BetweenFactor @@ -875,7 +873,7 @@ smoothing and mapping . Later in this document we will talk about how we can also use GTSAM to - do filtering (which you often do + do filtering (which often you do \emph on not \emph default @@ -928,7 +926,11 @@ Values \begin_layout Standard The latter point is often a point of confusion with beginning users of GTSAM. It helps to remember that when designing GTSAM we took a functional approach - of classes corresponding to mathematical objects, which are usually immutable. + of classes corresponding to mathematical objects, which are usually +\emph on +immutable +\emph default +. You should think of a factor graph as a \emph on function @@ -1027,7 +1029,7 @@ NonlinearFactorGraph \end_layout \begin_layout Standard -The relevant output from running the example is as follows: +The relevant output from running the example is as follows: \family typewriter \size small @@ -1546,7 +1548,7 @@ E(q)\define h(q)-m \end_inset -which is done on line 12. +which is done on line 14. Importantly, because we want to use this factor for nonlinear optimization (see e.g., \begin_inset CommandInset citation @@ -1599,11 +1601,11 @@ q_{y} \begin_inset Formula $q=\left(q_{x},q_{y},q_{\theta}\right)$ \end_inset -, yields the following simple +, yields the following \begin_inset Formula $2\times3$ \end_inset - matrix in tangent space which is the same the as the rotation matrix: + matrix: \end_layout \begin_layout Standard @@ -1620,6 +1622,110 @@ H=\left[\begin{array}{ccc} \end_layout +\begin_layout Paragraph* +Important Note +\end_layout + +\begin_layout Standard +Many of our users, when attempting to create a custom factor, are initially + surprised at the Jacobian matrix not agreeing with their intuition. + For example, above you might simply expect a +\begin_inset Formula $2\times3$ +\end_inset + + diagonal matrix. + This +\emph on +would +\emph default + be true for variables belonging to a vector space. + However, in GTSAM we define the Jacobian more generally to be the matrix + +\begin_inset Formula $H$ +\end_inset + + such that +\begin_inset Formula +\[ +h(qe^{\hat{\xi}})\approx h(q)+H\xi +\] + +\end_inset + +where +\begin_inset Formula $\xi=(\delta x,\delta y,\delta\theta)$ +\end_inset + + is an incremental update and +\begin_inset Formula $\exp\hat{\xi}$ +\end_inset + + is the +\series bold +exponential map +\series default + for the variable we want to update, In this case +\begin_inset Formula $q\in SE(2)$ +\end_inset + +, where +\begin_inset Formula $SE(2)$ +\end_inset + + is the group of 2D rigid transforms, implemented by +\series bold +\emph on +Pose2 +\emph default +. + +\series default +The exponential map for +\begin_inset Formula $SE(2)$ +\end_inset + + can be approximated to first order as +\begin_inset Formula +\[ +\exp\hat{\xi}\approx\left[\begin{array}{ccc} +1 & -\delta\theta & \delta x\\ +\delta\theta & 1 & \delta x\\ +0 & 0 & 1 +\end{array}\right] +\] + +\end_inset + +when using the +\begin_inset Formula $3\times3$ +\end_inset + + matrix representation for 2D poses, and hence +\begin_inset Formula +\[ +h(qe^{\hat{\xi}})\approx h\left(\left[\begin{array}{ccc} +\cos(q_{\theta}) & -\sin(q_{\theta}) & q_{x}\\ +\sin(q_{\theta}) & \cos(q_{\theta}) & q_{y}\\ +0 & 0 & 1 +\end{array}\right]\left[\begin{array}{ccc} +1 & -\delta\theta & \delta x\\ +\delta\theta & 1 & \delta x\\ +0 & 0 & 1 +\end{array}\right]\right)=\left[\begin{array}{c} +q_{x}+\cos(q_{\theta})\delta x-\sin(q_{\theta})\delta y\\ +q_{y}+\sin(q_{\theta})\delta x+\cos(q_{\theta})\delta y +\end{array}\right] +\] + +\end_inset + +which then explains the Jacobian +\begin_inset Formula $H$ +\end_inset + +. +\end_layout + \begin_layout Subsection Using Custom Factors \end_layout @@ -1680,13 +1786,13 @@ UnaryFactor \series default \emph default instances, and add them to graph. - GTSAM uses shared pointers to refer to factors in factor graphs, and + GTSAM uses shared pointers to refer to factors, and \series bold \emph on -boost::make_shared +emplace_shared \series default \emph default - is a convenience function to simultaneously construct a class and create + is a convenience method to simultaneously construct a class and create a \series bold \emph on @@ -1694,22 +1800,6 @@ shared_ptr \series default \emph default to it. - -\begin_inset Note Note -status collapsed - -\begin_layout Plain Layout -and on lines 4-6 we add three newly created -\series bold -\emph on -UnaryFactor -\series default -\emph default - instances to the graph. -\end_layout - -\end_inset - We obtain the factor graph from Figure \begin_inset CommandInset ref LatexCommand vref diff --git a/doc/gtsam.pdf b/doc/gtsam.pdf index d4cb8908f..6ea916d33 100644 Binary files a/doc/gtsam.pdf and b/doc/gtsam.pdf differ