参考文献：《Factor Graphs and GTSAM》原文链接

注：本文为原文的中文注释版，且有删减；绿色下划线部分为我不太理解的地方，可能是因为背后的数学逻辑还没掌握，也可能是我不会翻译

0.Overview-概论

Factor graphs are graphical models (Koller and Friedman, 2009) that are well suited to modeling complex estimation problems, such as Simultaneous Localization and Mapping (SLAM) or Structure from Motion (SFM). You might be familiar with another often used graphical model, Bayes networks, which are directed acyclic graphs. A factor graph, however, is a bipartite graph consisting of factors connected to variables. The variables represent the unknown random variables in the estimation problem, whereas the factors represent probabilistic constraints on those variables, derived from measurements or prior knowledge. In the following sections I will illustrate this with examples from both robotics and vision.

因子图是一种图形模型(Koller和Friedman, 2009)，非常适合于为复杂的估计问题建模，如同步定位和映射(SLAM)或运动结构(SFM)。您可能熟悉另一个经常使用的图形模型，贝叶斯网络，它是有向无环图。然而，因子图是由与变量相联系的因子组成的二部图。变量代表估计问题中未知的随机变量，而因子代表对这些变量的概率约束，由测量值或先验知识导出。在下面的章节中，我将用机器人和视觉的例子来说明这一点。

The GTSAM toolbox (GTSAM stands for “Georgia Tech Smoothing and Mapping”) toolbox is a BSD-licensed C++ library based on factor graphs, developed at the Georgia Institute of Technology by myself, many of my students, and collaborators. 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 development, visualization, and user interaction.

GTSAM工具箱(GTSAM代表“Georgia Tech Smoothing and Mapping”)是一个基于因子图的bsd许可的c++库，由我自己、我的许多学生和合作者在佐治亚理工学院开发。它为SLAM和SFM问题提供了最先进的解决方案，还可以用于建模和解决更简单和更复杂的估计问题。它还提供了一个MATLAB接口，允许快速原型开发、可视化和用户交互。

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 connected. This is exploited by the algorithms implemented in GTSAM to reduce computational complexity. Even when graphs are too dense to be handled efficiently by direct methods, GTSAM provides iterative methods that are quite efficient regardless.

GTSAM利用稀疏性来提高计算效率。通常情况下，测量只提供少数变量之间关系的信息，因此产生的因素图将是稀疏连接的。这被GTSAM中实现的算法所利用，以减少计算复杂度。即使图过于密集，无法通过直接方法有效地处理，GTSAM也提供了相当有效的迭代方法。

You can download the latest version of GTSAM from our Github repo.

您可以从我们的Github回购下载最新版本的GTSAM

1.因子图概念：

the factors represent probabilistic constraints on those variables, derived from measurements or prior knowledge

这些因素代表了对这些变量的概率约束，这些变量来自测量或先验知识

a factor graph, in which we only represent the unknown variables X1, X2, and X3, connected to factors that encode probabilistic information on them

因子图，其中我们只表示未知变量X1,X2,X3,与编码概率信息的因素相联系

To do maximum a-posteriori (MAP) inference, we then maximize the product

为了进行最大后验推理，我们将乘积最大化

the value of the factor graph. It should be clear from the figure that the connectivity of a factor graph encodes, for each factor fi, which subset of variables Xi it depends on

从图中可以清楚地看出因子图的连通性是编码的,每个fi是它依赖的Xi的子集

2.Modeling Robot Motion

2.1举例：

There are three variables x1, x2, and x3 which represent the poses of the robot over time, rendered in the figure by the open-circle variable nodes

这里有三个变量x1，x2，x3，分别代表了机器人随时间变化的三个阶段的位姿，在图像中表现为开环变量节点

In this example, we have one unary factor f0(x1) on the first pose x1 that encodes our prior knowledge about x1, and two binary factors that relate successive poses, respectively f1(x1,x2;o1)f and f2(x2,x3;o2), where o1 and o2 represent odometry measurements.

在这个例子中，我们有一个一元因子，其位于第一个位姿x1上，表示关于x1的先验；还有两个二元因子，与其相关联的连续位姿所联系，分别是 f1(x1,x2;o1)f 和 f2(x2,x3;o2)，其中的o1，o2代表里程计测量值

The following C++ code, included in GTSAM as an example，creates the factor graph in above Figure

下面的用GTSAM库的c++代码是对上述因子图的构建

// Create an empty nonlinear factor graph
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.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));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

Above, line 2 creates an empty factor graph. We then add the factor f0(x1) on lines 5-8 as an instance of PriorFactor<T>, a templated class provided in the slam subfolder, with T=Pose2. Its constructor takes a variable Key (in this case 1), a mean of type Pose2, created on Line 5, and a noise model for the prior density. We provide a diagonal Gaussian of type noiseModel::Diagonal by specifying three standard deviations in line 7, respectively 30 cm. on the robot's position, and 0.1 radians on the robot's orientation. Note that the Sigmas constructor returns a shared pointer, anticipating that typically the same noise models are used for many different factors.

上面，第二行创建了一个空的因子图，然后我们在5-8行添加因子f0(x1)作为模板类PriorFactor<T>的实例，它是一个slam子文件提供的模板类，这里T是Pose2 ，该模板类的构造函数需要一个Key、一个与Pose2格式相同的均值、一个针对于先验密度的噪声模型，我们提供了一个noiseModel::Diagonal 的diagonal Gaussian，其值是三个标准差，分别为30cm（机器人位姿），0.1弧度（机器人方向）。注意Sigmas 的构造函数返回一个共享指针，预期相同的噪音模型会用于不同的因子

Similarly, odometry measurements are specified as Pose2 on line 11, with a slightly different noise model defined on line 12-13. We then add the two factors f1(x1,x2;o1)and f2(x2,x3;o2)on lines 14-15, as instances of yet another templated class, BetweenFactor<T>, again with T=Pose2.

相似地，里程计测量值在第11行被定义为Pose2 ，并伴有一个与之前稍微不等的噪音模型，在12-13行定义，然后我们在14-15行添加两个因子f1(x1,x2;o1)和f2(x2,x3;o2）,作为另外一个模板类BetweenFactor<T>的实例，这里同样T设置为Pose2

When running the example (make OdometryExample.run on the command prompt), it will print out the factor graph as follows:

当运行这个实例（........），将会打印出因子图如下：

Factor Graph:
size: 3
Factor 0: PriorFactor on 1prior 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)noise model: diagonal sigmas [0.2; 0.2; 0.1];

2.2Factor Graphs versus Values:

通过上面论述得到两个启发：

1.The factor graph and its embodiment in code specify the joint probability distribution P(X|Z)over the entire trajectory X={x1,x2,x3} of the robot, rather than just the last pose. This smoothing view of the world gives GTSAM its name: “smoothing and mapping”. Later in this document we will talk about how we can also use GTSAM to do filtering (which you often do not want to do) or incremental inference (which we do all the time).

因子图和它的代码实体定义了一个在机器人整个运动轨迹X={x1，x2，x3}上位姿的联合概率分布P(X|Z)，而不是仅对于最后一个位姿，真是这种平滑的视角给予了GTSAM的名字“smoothing and mapping”，后续会讨论用GTSAM去做滤波（虽然我们通常不想这么做）或者增量推理运算（我们通常的做法）

2.A factor graph in GTSAM is just the specification of the probability density P(X|Z), and the corresponding FactorGraph class and its derived classes do not ever contain a “solution”. Rather, there is a separate type Values that is used to specify specific values for (in this case) x1, x2, and x3, which can then be used to evaluate the probability (or, more commonly, the error) associated with particular values.

GTSAM的因子图仅仅是对概率密度P(X|Z)的说明，以及所对应的因子图类-FactorGraph class和它的派生类，他们从不包含“解决方案”，相反，这里有一系列不同的Values需要被赋予特定的值，比如这里的x1，x2，x3.这些会被用于评估与特定值有关的概率（更通常的说法是误差）

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. You should think of a factor graph as a function to be applied to values -as the notation f(X)∝P(X|Z)f(X)∝P(X|Z) implies- rather than as an object to be modified.

后面这种观点对于GTSAM的初学者来说是具有困惑性的，但它帮助我们认识到当我们设计GTSAM时我们采用了a functional approach of classes corresponding to mathematical objects，这通常是不可变的，你应该将因子图看作为一种应用于一些值上的函数，正如公式 f(X)∝P(X|Z)f(X)∝P(X|Z)所暗示的一样，而不是看作一个需要被修正的对象

2.3Non-linear Optimization in GTSAM-非线性优化

The listing below creates a Values instance, and uses it as the initial estimate to find the maximum a-posteriori (MAP) assignment for the trajectory X:

下面列出表是创建了一个Values的实例，并且用这个初始预测去求整个轨迹X的最大后验概率(MAP)

// create (deliberately inaccurate) initial estimate
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));// optimize using Levenberg-Marquardt optimization
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

Lines 2-5 in Listing 2.4 create the initial estimate, and on line 8 we create a non-linear Levenberg-Marquardt style optimizer, and call optimize using default parameter settings. The reason why GTSAM needs to perform non-linear optimization is because the odometry factors f1(x1,x2;o1) and f2(x2,x3;o2)are non-linear, as they involve the orientation of the robot. This also explains why the factor graph we created in Listing 2.2 is of type NonlinearFactorGraph. The optimization class linearizes this graph, possibly multiple times, to minimize the non-linear squared error specified by the factors.

2-5行创建了初始预测，第8行我们创建了非线性LM形式的优化器，并调用使用默认参数的optimize方法进行优化，为什么GTSAM需要运行非线性优化方法是因为里程计因子f1，f2是非线性的，因为它们涉及机器人的方向，这也解释了为什么之前创建的因子图被称作非线性因子图，优化类线性化这种图，可能要进行多次，去最小化被因子定义的非线性平方误差

The relevant output from running the example is as follows:

运行结果如下：

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)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)

It can be seen that, subject to very small tolerance, the ground truth solution x1=(0,0,0), x2=(2,0,0), and x3=(4,0,0)is recovered.

由结果可知，在小范围的容忍度下，真实值方案x1=(0,0,0), x2=(2,0,0), and x3=(4,0,0)被恢

2.4Full Posterior Inference-全后验推理

GTSAM can also be used to calculate the covariance matrix for each pose after incorporating the information from all measurements Z. Recognizing that the factor graph encodes the posterior density P(X|Z), the mean μ together with the covariance Σ for each pose x approximate the marginal posterior density P(x|Z). Note that this is just an approximation, as even in this simple case the odometry factors are actually non-linear in their arguments, and GTSAM only computes a Gaussian approximation to the true underlying posterior.

GTSAM还可以在得到所有Z的测量信息后为每个位姿x计算协方差矩阵，因子图是对后验概率P(X|Z)的表征，而μ、Σ近似于边缘概率密度P(x|Z)的表征，注意到这仅仅是一种近似，因为即便是这里简单的举例中里程计因子也都是非线性的，GTSAM计算的仅仅是真实后验概率分布的高斯近似

The following C++ code will recover the posterior marginals:

下面的c++代码将恢复后验边缘概率密度分布:

// Query the marginals
cout.precision(2);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;

The relevant output from running the example is as follows:

3 Robot Localization-机器人定位

3.1 Unary Measurement Factors-一元测量因子

Robot localization factor graph with unary measurement factors at each time step.

机器人定位因子图（每个时间点都有一个一元测量因子）

In particular, we use unary measurement factors to handle external measurements. The example from Section 2 is not very useful on a real robot, because it only contains factors corresponding to odometry measurements. These are imperfect and will lead to quickly accumulating uncertainty on the last robot pose, at least in the absence of any external measurements (see Section 2.5). Figure 4 shows a new factor graph where the prior f0(x1)is omitted and instead we added three unary factors f1(x1;z1), f2(x2;z2), and f3(x3;z3), one for each localization measurement zt, respectively. Such unary factors are applicable for measurements zt that depend only on the current robot pose, e.g., GPS readings, correlation of a laser range-finder in a pre-existing map, or indeed the presence of absence of ceiling lights (see Dellaert et al. (1999) for that amusing example).

特别地，我们使用一元测量因子去处理外部测量，相对于第二章节的例子中只包含相邻里程计测量之间的因子，而没有任何的外部测量，这是不完美的会导致不确定性快速累积至最后一个机器人位姿。上图创建了一个新的因子图，在新的因子图中f0(x1)被移除，作为替代三个一元因子f1(x1;z1), f2(x2;z2), f3(x3;z3)被加入，每一个因子是对机器人定位的测量值Zt，这样的一元因子仅仅依赖于当前的机器人位姿如gps定位，激光雷达数据制成的预先地图等。

3.2 Defining Custom Factors-定义定制因子

In GTSAM, you can create custom unary factors by deriving a new class from the built-in class NoiseModelFactor1<T>, which implements a unary factor corresponding to a measurement likelihood with a Gaussian noise model：

where m is the measurement, q is the unknown variable, h(q) is a (possibly nonlinear) measurement function, and Σ is the noise covariance. Note that m is considered known above, and the likelihood L(q;m) will only ever be evaluated as a function of q, which explains why it is a unary factor f(q). It is always the unknown variable q that is either likely or unlikely, given the measurement.

在GTSAM中，你可以创建一个定制的一元因子，通过继承一个内建类NoiseModelFactor1<T>，这个内建类实现了一个与测量值的可能性度量相关的一元因子（以高斯噪声模型定义），m是测量值，q是一个未知变量，h(q)是一个（非线性）度量函数，Σ是一个噪声协方差，注意到m是已知的，可能性度量L(q;m)仅以q为自变量，这也解释了f(q)为什么是一元因子，基于测量值，总可以得到q要么是可能要么是不可能

Note: many people get this backwards, often misled by the conditional density notation P(m|q). In fact, the likelihood L(q;m) is defined as any function of q proportional to P(m|q)

注意：许多人都弄反了，经常被条件概率公式P(m|q)所误导，实际上，可能性度量L(q;m)被定义为正比于P(m|q)的函数

Listing below shows an example on how to define the custom factor class UnaryFactor which implements a “GPS-like” measurement likelihood:

下表显示了一个例子告诉我们如何定义一个定制类UnaryFactor，它实现了类似于GPS的测量值的可能性度量

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

In defining the derived class on line 1, we provide the template argument Pose2 to indicate the type of the variable q, whereas the measurement is stored as the instance variables mx_ and my_, defined on line 2. The constructor on lines 5-6 simply passes on the variable key j and the noise model to the superclass, and stores the measurement values provided. The most important function to has be implemented by every factor class is evaluateError, which should return E(q)=h(q)−m which is done on line 12. Importantly, because we want to use this factor for nonlinear optimization (see e.g., Dellaert and Kaess 2006 for details), whenever the optional argument H is provided, a Matrix reference, the function should assign the Jacobian of h(q) to it, evaluated at the provided value for q. This is done for this example on line 11. In this case, the Jacobian of the 2-dimensional function h, which just returns the position of the robot,

在第1行定义了继承类，我们提供了模板申明为Pose2，暗示变量q的类型，然而测量值被存储为实例变量mx_ and my_，在第2行定义的。5-6行的构造函数仅仅传递了变量键值j、超类的噪声模型、存储提供的测量值，每个因子类必须要实现的最重要的函数是evaluateError，它应该返回 E(q)=h(q)−m，在12行定义，重要的是，因为我们想利用这个因子进行非线性优化，任何时候可选参数H一旦给定了，函数就应该计算h(q)的雅可比矩阵即对q值的评估，这在11行实现，在这个例子中，函数h(q)的雅可比矩阵是二维的,它就是返回了机器人的位姿

with respect the 3-dimensional pose q=(qx,qy,qθ), yields the following 2×3 matrix:

如果q是三维的q=(qx,qy,qθ)，则产生的H为：

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 2×3 diagonal matrix. This would be true for variables belonging to a vector space. However, in GTSAM we define the Jacobian more generally to be the matrix H such that：

我们很多使用者每当尝试去创建一个定制因子，最初都会惊讶于雅可比矩阵与直觉的违背。比如，上面你可能期望得到的雅可比矩阵是一个2×3的对角矩阵，如果我们的变量是属于向量空间那会是如此。但是，在GTSAM中我们定义的雅可比矩阵通常形如：

where ξ=(δx,δy,δθ) is an incremental update and expˆξ is the exponential map for the variable we want to update, In this case q∈SE(2), where SE(2) is the group of 2D rigid transforms, implemented by Pose2. The exponential map for SE(2) can be approximated to first order as：

这里面ξ=(δx,δy,δθ)是一个渐进式的更新，expˆξ 是ξ（我们想更新的变量）的指数映射，这个例子中 q∈SE(2)，而SE(2)的指数映射可被近似为如下：

when using the 3×3 matrix representation for 2D poses, and hence：

当使用3×3矩阵代表我们的2D位姿时，因此得到：

which then explains the Jacobian H.

这解释了我们H的形式（2×1而不是2×3）

3.3 Using Custom Factors-使用定制因子

The following C++ code fragment illustrates how to create and add custom factors to a factor graph:

下面的c++代码片段阐释了怎么去创建并添加一个定制因子到一个因子图：

// add unary measurement factors, like GPS, on all three poses
noiseModel::Diagonal::shared_ptr 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));

In Listing above, we create the noise model on line 2-3, which now specifies two standard deviations on the measurements mx and my. On lines 4-6 we create shared_ptr versions of three newly created UnaryFactor instances, and add them to graph. GTSAM uses shared pointers to refer to factors in factor graphs, and boost::make_shared is a convenience function to simultaneously construct a class and create a shared_ptr to it. We obtain the factor graph from Figure 4.

在上表中，我们在第2-3行创建了一个噪声模型，定义了两个对于mx，my的标准差，在4-6行创建了三个以shared_ptr为形式的一元因子 UnaryFactor 实例，并添加到因子图中。GTSAM使用共享指针去引用因子图中的因子，GTSAM使用共享指针引用因子图中的因子，使用boost::make_shared，它是一个 convenience function，实现同时构建一个类和一个共享指针，于是我们得到了像Figure 4的因子图。

3.4 Full Posterior Inference

The three GPS factors are enough to fully constrain all unknown poses and tie them to a “global” reference frame, including the three unknown orientations. If not, GTSAM would have exited with a singular matrix exception. The marginals can be recovered exactly as in Section 2.4, and the solution and marginal covariances are now given by the following:

这三个GPS因子足够去约束所有未知的位姿并且将他们绑定到一个包含三个未知方向的全局参考系，如果不这样，GTSAM会有一个 0 矩阵异常，边缘值可以像2.4节那样被恢复，结果和边缘协方差现在给出如下：

Final Result:
Values with 3 values:
Value 1: (-1.5e-14, 1.3e-15, -1.4e-16)
Value 2: (2, 3.1e-16, -8.5e-17)
Value 3: (4, -6e-16, -8.2e-17)x1 covariance:0.0083      4.3e-19     -1.1e-184.3e-19       0.0094      -0.0031-1.1e-18      -0.0031       0.0082
x2 covariance:0.0071      2.5e-19     -3.4e-192.5e-19       0.0078      -0.0011-3.4e-19      -0.0011       0.0082
x3 covariance:0.0083     4.4e-19     1.2e-184.4e-19      0.0094      0.00311.2e-18      0.0031       0.018

Comparing this with the covariance matrices in Section 2.4 we can see that the uncertainty no longer grows without bounds as measurement uncertainty accumulates. Instead, the “GPS” measurements more or less constrain the poses evenly, as expected.

与2.4节的协方差矩阵比较我们可以看出不确定性不在增长当没有了边界测量值，作为替代地，GPS测量值或多或少限制了平均位姿，像期望的那样。

It helps a lot when we view this graphically, as in Figure 5, where I show the marginals on position as 5-sigma covariance ellipses that contain 99.9996% of all probability mass. For the odometry marginals, it is immediately apparent from the figure that (1) the uncertainty on pose keeps growing, and (2) the uncertainty on angular odometry translates into increasing uncertainty on y. The localization marginals, in contrast, are constrained by the unary factors and are all much smaller. In addition, while less apparent, the uncertainty on the middle pose is actually smaller as it is constrained by odometry from two sides.

这个图很形象，在这个图中，where I show the marginals on position as 5-sigma covariance ellipses that contain 99.9996% of all probability mass。对于里程计边缘值，可以立即清晰地看到位姿不确定性持续增长，但是角度的不确定性被转化为依赖y的不确定性增长，定位边缘值相反地，被一元因子所约束都很小。此外，不显而易见的有，在中间位置的位姿的不确定性实际上比两个边界处的要小。

4 PoseSLAM

4.1 Loop Closure Constraints

The simplest instantiation of a SLAM problem is PoseSLAM, which avoids building an explicit map of the environment. The goal of SLAM is to simultaneously localize a robot and map the environment given incoming sensor measurements (Durrant-Whyte and Bailey, 2006). Besides wheel odometry, one of the most popular sensors for robots moving on a plane is a 2D laser-range finder, which provides both odometry constraints between successive poses, and loop-closure constraints when the robot re-visits a previously explored part of the environment.

最简单的SLAM实例是位姿SLAM（PoseSLAM），它不用建立明确的环境地图，SLAM的目标是实时定位机器人并依据输入的传感器测量数据构建周围环境的地图。此外轮式里程计（一种最有名的用于机器人平面移动的2D雷达探测器），它提供了连续位姿间的里程计约束以及回环约束当机器人再次访问之前探测到的地方。

A factor graph example for PoseSLAM is shown in Figure 6. The following C++ code, included in GTSAM as an example, creates this factor graph in code:

上图表示的是一个PoseSLAM的因子图例子，包含GTSAM作为示例，代码如下：

NonlinearFactorGraph graph;
noiseModel::Diagonal::shared_ptr priorNoise =noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));// Add odometry factors
noiseModel::Diagonal::shared_ptr model =noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));// Add the loop closure constraint
graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));

As before, lines 1-4 create a nonlinear factor graph and add the unary factor f0(x1). As the robot travels through the world, it creates binary factors ft(xt,xt+1) corresponding to odometry, added to the graph in lines 6-12 (Note that M_PI_2 refers to pi/2). But line 15 models a different event: a loop closure. For example, the robot might recognize the same location using vision or a laser range finder, and calculate the geometric pose constraint to when it first visited this location. This is illustrated for poses x5 and x2, and generates the (red) loop closing factor f5(x5,x2).

像之前一样，1-4行创建了一个非线性因子图并将一元因子添加到图中。当机器人穿越场地时，会创建和里程计相关联的二元因子ft(xt,xt+1)，在6-12行添加到图中（M_PI_2 指的是 pi/2）。但是第15行添加了一个新事件：回环。例如，机器人可能利用视觉或者雷达意识到自己处于与之前某时刻相同的定位，并计算几何位姿约束。就像x5和x2阐释的那样，并生成红色的回环闭合因子f5(x5,x2)

We can optimize this factor graph as before, by creating an initial estimate of type Values, and creating and running an optimizer. The result is shown graphically in Figure 7, along with covariance ellipses shown in green. These 5-sigma covariance ellipses in 2D indicate the marginal over position, over all possible orientations, and show the area which contain 99.9996% of the probability mass. The graph shows in a clear manner that the uncertainty on pose x5 is now much less than if there would be only odometry measurements. The pose with the highest uncertainty, x4, is the one furthest away from the unary constraint f0(x1), which is the only factor tying the graph to a global coordinate frame.

我们可以像以前一样，通过创建类型Values的初始估计，并创建和运行优化器来优化这个因子图。结果如图7所示，还可见绿色的协方差椭圆。这些2D中的5-标准差椭圆表示所有边缘位置值和所有可能的方向，并显示包含99.9996%概率质量的区域，这张图清楚地表明，现在x5位姿的不确定度比只有里程数测量时要小得多。具有最高不确定性的位姿x4是离一元约束f0(x1)最远的位姿，这是将图形绑定到全局坐标系的唯一因素。

5 Landmark-based SLAM

5.1 Basics

In landmark-based SLAM, we explicitly build a map with the location of observed landmarks, which introduces a second type of variable in the factor graph besides robot poses. An example factor graph for a landmark-based SLAM example is shown in Figure 10, which shows the typical connectivity: poses are connected in an odometry Markov chain, and landmarks are observed from multiple poses, inducing binary factors. In addition, the pose x1has the usual prior on it.

在基于位姿的SLAM中，我们显式地构建一个带有观察到的地标位置的地图，它引入了机器人姿态之外的第二种变量。一个基于地标的SLAM实例的因子图如图10所示，它显示了典型的连通性:机器人位姿在里程计马尔可夫链中相连接，地标被多个位姿处所观察到，两者之间用二维因子所连接，此外x1 处和往常一样有个先验。

The factor graph from Figure 10 can be created using the MATLAB code in Listing 5.1. As before, on line 2 we create the factor graph, and Lines 8-18 create the prior/odometry chain we are now familiar with. However, the code on lines 20-25 is new: it creates three measurement factors, in this case “bearing/range” measurements from the pose to the landmark.

可以用表5.1的代码构建如图10的因子图，和以前一样，第2行我们创建一个因子图，8-18行创建先验、里程计链（我们都很熟悉了）。然而，在20-25行是新的对我们来说，他创建3个测量因子： “bearing/range” 由位姿指向路标

% Create graph container and add factors to it
graph = NonlinearFactorGraph;% Create keys for variables
i1 = symbol('x',1); i2 = symbol('x',2); i3 = symbol('x',3);
j1 = symbol('l',1); j2 = symbol('l',2);% Add prior
priorMean = Pose2(0.0, 0.0, 0.0); % prior at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.1]);
% add directly to graph
graph.add(PriorFactorPose2(i1, priorMean, priorNoise));% Add odometry
odometry = Pose2(2.0, 0.0, 0.0);
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.1]);
graph.add(BetweenFactorPose2(i1, i2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(i2, i3, odometry, odometryNoise));% Add bearing/range measurement factors
degrees = pi/180;
brNoise = noiseModel.Diagonal.Sigmas([0.1; 0.2]);
graph.add(BearingRangeFactor2D(i1, j1, Rot2(45*degrees), sqrt(8), brNoise));
graph.add(BearingRangeFactor2D(i2, j1, Rot2(90*degrees), 2, brNoise));
graph.add(BearingRangeFactor2D(i3, j2, Rot2(90*degrees), 2, brNoise));

5.2 Of Keys and Symbols

The only unexplained code is on lines 4-6: here we create integer keys for the poses and landmarks using the symbol function. In GTSAM, we address all variables using the Key type, which is just a typedef to size_t ^2 ,The keys do not have to be numbered continuously, but they do have to be unique within a given factor graph. For factor graphs with different types of variables, we provide the symbol function in MATLAB, and the Symbol type in C++, to help you create (large) integer keys that are far apart in the space of possible keys, so you don't have to think about starting the point numbering at some arbitrary offset. To create a a symbol key you simply provide a character and an integer index. You can use base 0 or 1, or use arbitrary indices: it does not matter. In the code above, we we use 'x' for poses, and 'l' for landmarks.

唯一无法解释的代码在第4-6行:这里我们使用symbol函数为姿势和地标创建整数键。在GTSAM中，我们使用Key类型来处理所有变量，它只是size_t^2的类型定义,键不必连续编号，但它们在给定的因子图中必须是唯一的。对于具有不同类型变量的因子图，我们在MATLAB中提供了符号函数，在c++中提供了符号类型，以帮助您创建在可能键空间中相距很远的导致整数很大的键，因此您不必考虑从某个任意偏移点开始编号。要创建符号键，只需提供一个字符和一个整数索引。您可以使用以0或1为底，或使用任意索引:这无关紧要。在上面的代码中，我们使用'x'表示姿态,'l'表示路标。

Factor Graphs and GTSAM相关推荐

视觉SLAM学习（三）--------SLAM 综述
SLAM概述参考资料分享来自本人博客:https://blog.csdn.net/Darlingqiang/article/details/78840931 SLAM一般处理流程包括track和ma ...
visual tree view在工具箱力没有_Visual-Inertial Odometry
1. 定义 "Visual-Inertial Odometry",俗称VIO,是一个使用一个或者多个相机.一个或者多个IMU(Inertial Measurement Units) ...
最详细的SLAM综述
论文:Past, Present, and Future of Simultaneous Localization And Mapping: Towards the Robust-Perception ...
SLAM领域国内外优秀实验室汇总
1. 美国卡耐基梅陇大学机器人研究所研究方向:机器人感知.结构,服务型.运输.制造业.现场机器研究所主页:https://www.ri.cmu.edu/ 下属 Field Robotic Cent ...
公开课精华|机器人的带约束轨迹规划
本文章总结于大疆前技术总监,目前在卡内基梅隆大学读博的杨硕博士在深蓝学院的关于机器人的带约束轨迹规划的公开课演讲内容. --------全文约5000字-------- 笔者不是机器人领域的,因此特地 ...
SLAM综述性论文阅读手记
==1 简介== 古典年代(1986-2004):这一时期,引入了SLAM概率论推导方法,包括基于扩展卡尔曼滤波.粒子滤波和最大似然估计.第一个挑战是效率和数据关联(求解位姿)的鲁棒性问题. 算法分析 ...
【GTSAM】GTSAM学习
1 what GTSAM ? GTSAM 是一个在机器人领域和计算机视觉领域用于平滑(smoothing)和建图(mapping)的C++库.它与g2o不同的是,g2o采用稀疏矩阵的方式求解一个非线性 ...
gtsam Overview
gtsam Overview GTSAM 库具有构建因子图表示和优化所需的三个主要组件,用户需要适应他们的特定问题. FactorGraph 因子图包含一组要求解的变量(即机器人姿势.地标姿势等)以及 ...
【GTSAM】GTSAM/iSAM1/2资源整理
1 what GTSAM ? GTSAM 是一个在机器人领域和计算机视觉领域用于平滑(smoothing)和建图(mapping)的C++库.它与g2o不同的是,g2o采用稀疏矩阵的方式求解一个非线性 ...
计算机视觉 | 计算机视觉相关算法及工具
博主github:https://github.com/MichaelBeechan 博主CSDN:https://blog.csdn.net/u011344545 计算机视觉数据集:https:// ...

Factor Graphs and GTSAM