Merge pull request #1139 from borglab/fix/doc
Frank Dellaert
GitHub
commit
134a6b819b
|
|
@ -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.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
|
||||
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
|
||||
graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
|
||||
graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
|
||||
graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
|
||||
|
||||
|
|
|
|||
|
|
@ -6,8 +6,7 @@ public:
|
|||
NoiseModelFactor1<Pose2>(model, j), mx_(x), my_(y) {}
|
||||
|
||||
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,
|
||||
|
|
|
|||
|
|
@ -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<Pose2>(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<Pose2>(1, 2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
|
||||
|
|
|
|||
|
|
@ -1,11 +1,14 @@
|
|||
Factor Graph:
|
||||
size: 3
|
||||
factor 0: PriorFactor on 1
|
||||
|
||||
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)
|
||||
|
||||
Factor 1: BetweenFactor(1,2)
|
||||
measured: (2, 0, 0)
|
||||
noise model: diagonal sigmas [0.2; 0.2; 0.1];
|
||||
factor 2: BetweenFactor(2,3)
|
||||
|
||||
Factor 2: BetweenFactor(2,3)
|
||||
measured: (2, 0, 0)
|
||||
noise model: diagonal sigmas [0.2; 0.2; 0.1];
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
252
doc/gtsam.lyx
252
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
|
||||
|
|
@ -154,18 +150,14 @@ filename "common_macros.tex"
|
|||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Section*
|
||||
Overview
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_layout Abstract
|
||||
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
|
||||
\begin_layout Abstract
|
||||
|
||||
\series bold
|
||||
Factor graphs
|
||||
|
|
@ -206,7 +198,7 @@ ts or prior knowledge.
|
|||
robotics and vision.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
\begin_layout Abstract
|
||||
The GTSAM toolbox (GTSAM stands for
|
||||
\begin_inset Quotes eld
|
||||
\end_inset
|
||||
|
|
@ -221,11 +213,13 @@ 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
|
||||
\begin_layout Abstract
|
||||
GTSAM exploits sparsity to be computationally efficient.
|
||||
Typically measurements only provide information on the relationship between
|
||||
a handful of variables, and hence the resulting factor graph will be sparsely
|
||||
|
|
@ -236,14 +230,17 @@ l complexity.
|
|||
GTSAM provides iterative methods that are quite efficient regardless.
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
You can download the latest version of GTSAM at
|
||||
\begin_layout Abstract
|
||||
You can download the latest version of GTSAM from GitHub at
|
||||
\end_layout
|
||||
|
||||
\begin_layout Abstract
|
||||
\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 +738,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 +770,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 +792,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 +801,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 +872,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 +925,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
|
||||
|
|
@ -1363,14 +1364,18 @@ where
|
|||
\end_inset
|
||||
|
||||
is the measurement,
|
||||
\begin_inset Formula $q$
|
||||
\begin_inset Formula $q\in SE(2)$
|
||||
\end_inset
|
||||
|
||||
is the unknown variable,
|
||||
\begin_inset Formula $h(q)$
|
||||
\end_inset
|
||||
|
||||
is a (possibly nonlinear) measurement function, and
|
||||
is a
|
||||
\series bold
|
||||
measurement function
|
||||
\series default
|
||||
, and
|
||||
\begin_inset Formula $\Sigma$
|
||||
\end_inset
|
||||
|
||||
|
|
@ -1546,7 +1551,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 +1604,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
|
||||
|
|
@ -1618,6 +1623,171 @@ H=\left[\begin{array}{ccc}
|
|||
\end_inset
|
||||
|
||||
|
||||
\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
|
||||
|
||||
identity 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(q\exp\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 y\\
|
||||
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 y\\
|
||||
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 Standard
|
||||
Lie groups are very relevant in the robotics context, and you can read more
|
||||
here:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
\begin_inset Flex URL
|
||||
status open
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
https://github.com/borglab/gtsam/blob/develop/doc/LieGroups.pdf
|
||||
\end_layout
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
\begin_inset Flex URL
|
||||
status open
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
https://github.com/borglab/gtsam/blob/develop/doc/math.pdf
|
||||
\end_layout
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Standard
|
||||
In some cases you want to go even beyond Lie groups to a looser concept,
|
||||
|
||||
\series bold
|
||||
manifolds
|
||||
\series default
|
||||
, because not all unknown variables behave like a group, e.g., the space of
|
||||
3D planes, 2D lines, directions in space, etc.
|
||||
For manifolds we do not always have an exponential map, but we have a retractio
|
||||
n that plays the same role.
|
||||
Some of this is explained here:
|
||||
\end_layout
|
||||
|
||||
\begin_layout Itemize
|
||||
\begin_inset Flex URL
|
||||
status open
|
||||
|
||||
\begin_layout Plain Layout
|
||||
|
||||
https://gtsam.org/notes/GTSAM-Concepts.html
|
||||
\end_layout
|
||||
|
||||
\end_inset
|
||||
|
||||
|
||||
\end_layout
|
||||
|
||||
\begin_layout Subsection
|
||||
|
|
@ -1680,13 +1850,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 +1864,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
|
||||
|
|
|
|||
BIN
doc/gtsam.pdf
BIN
doc/gtsam.pdf
Binary file not shown.
|
|
@ -134,7 +134,7 @@ Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Gaussian::print(const string& name) const {
|
||||
gtsam::print(thisR(), name + "Gaussian");
|
||||
gtsam::print(thisR(), name + "Gaussian ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -285,7 +285,7 @@ Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Diagonal::print(const string& name) const {
|
||||
gtsam::print(sigmas_, name + "diagonal sigmas");
|
||||
gtsam::print(sigmas_, name + "diagonal sigmas ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -355,8 +355,8 @@ bool Constrained::constrained(size_t i) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Constrained::print(const std::string& name) const {
|
||||
gtsam::print(sigmas_, name + "constrained sigmas");
|
||||
gtsam::print(mu_, name + "constrained mu");
|
||||
gtsam::print(sigmas_, name + "constrained sigmas ");
|
||||
gtsam::print(mu_, name + "constrained mu ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue