Updated everything up to and including custom factor, including more explanation about H

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<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
+graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
-graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
+graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
-graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
+graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
 

doc/Code/LocalizationFactor.cpp

@@ -1,13 +1,12 @@
 class UnaryFactor: public NoiseModelFactor1<Pose2> {
   double mx_, my_; ///< X and Y measurements
-
+  
 public:
   UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
     NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
 
-  Vector evaluateError(const Pose2& q,
+  Vector evaluateError(const Pose2& q, 
-                       boost::optional<Matrix&> H = boost::none) const
+      boost::optional<Matrix&> H = boost::none) const override {
-  {
     const Rot2& R = q.rotation();
     if (H) (*H) = (gtsam::Matrix(2, 3) <<
             R.c(), -R.s(), 0.0,

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 =
+auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
-  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
-graph.addPrior(1, priorMean, priorNoise);
 
 // Add two odometry factors
 Pose2 odometry(2.0, 0.0, 0.0);
-noiseModel::Diagonal::shared_ptr odometryNoise =
+auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
-  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

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)
+Factor 1: BetweenFactor(1,2)
-  noise model: diagonal sigmas [0.2; 0.2; 0.1];
+  measured:  (2, 0, 0)
-factor 2: BetweenFactor(2,3)
-  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];

doc/Code/OdometryOutput2.txt

@@ -1,11 +1,23 @@
 Initial Estimate:
+
 Values with 3 values:
-Value 1: (0.5, 0, 0.2)
+Value 1: (gtsam::Pose2)
-Value 2: (2.3, 0.1, -0.2)
+(0.5, 0, 0.2)
-Value 3: (4.1, 0.1, 0.1)
+
+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 1: (gtsam::Pose2)
-Value 2: (2, 7.4e-18, -2.5e-18)
+(7.5-16, -5.3-16, -1.8-16)
-Value 3: (4, -1.8e-18, -3.1e-18)
+
+Value 2: (gtsam::Pose2)
+(2, -1.1-15, -2.5-16)
+
+Value 3: (gtsam::Pose2)
+(4, -1.7-15, -2.5-16)

doc/Code/OdometryOutput3.txt

@@ -1,12 +1,12 @@
 x1 covariance:
-       0.09     1.1e-47     5.7e-33
+   0.09 1.7e-33 2.8e-33
-    1.1e-47        0.09     1.9e-17
+1.7e-33    0.09 2.6e-17
-    5.7e-33     1.9e-17        0.01
+2.8e-33 2.6e-17    0.01
 x2 covariance:
-       0.13     4.7e-18     2.4e-18
+   0.13 1.2e-18 6.1e-19
-    4.7e-18        0.17        0.02
+1.2e-18    0.17    0.02
-    2.4e-18        0.02        0.02
+6.1e-19    0.02    0.02
 x3 covariance:
-       0.17     2.7e-17     8.4e-18
+   0.17 8.6e-18 2.7e-18
-    2.7e-17        0.37        0.06
+8.6e-18    0.37    0.06
-    8.4e-18        0.06        0.03
+2.7e-18    0.06    0.03

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
+ It also provides MATLAB and Python wrappers which allow for rapid prototype
-t, visualization, and user interaction.
+ 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<T>
@@ -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<T>
@@ -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