GTSAM学习：：Example（1）SimpleRotation

Example（1）SimpleRotation

---

这是个超级简单的例子，根据一个先验信息来优化一个旋转角度。

这个示例将对带有单个因子的单个变量执行一个相对简单的优化。

 

引入头文件

---

1. 2D旋转

#include <gtsam/geometry/Rot2.h>

2. 符号表示

系统中的每个变量(pose)都必须使用一个惟一的key进行标识，我们可以使用keys (1, 2, 3, ...) or symbols (X1, X2, L1)，这里使用symbols

#include <gtsam/inference/Symbol.h>

3. 先验因子

在GTSAM中，测量函数用“因子”表示，并且gtsam已经集成了一些通用的因子。

#include <gtsam/slam/PriorFactor.h>

4. 非线性因子图

创建因子后，我们将把它们添加到因子图中，因为我们创建的因子是非线性的，所以需要一个非线性因子图

#include <gtsam/nonlinear/NonlinearFactorGraph.h>

5. 值

GTSAM中的非线性求解器是迭代求解器，这意味着它们在初始值附近对非线性部分进行线性化，然后求解、更新，直到收敛，因此这需要我们未每个待优化变量指定一个初始的估计值。

#include <gtsam/nonlinear/Values.h>

6. LM求解器

最后，当所有因子都加入到因子图中，就需要使用求解器来求解。

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

 

命名空间

---

using namespace std;
using namespace gtsam;

const double degree = M_PI / 180;

 

int main()

---

1. 创建因子

首先创建一个 一元边约束来表示因子，在这个例子中，先验是传感器的测量，该测量带有噪声。

这里创建的“键”是一个标签，用于将状态的一部分(存储在“RotValues”中)与特定因子关联起来，它们需要一个索引便于查找，并且是唯一的。

通常，创建一个因子需要：

• 待优化的变量，key或者symbol
• 测量值
• 测量模型（维度与因子匹配）

  // 先验：测量值，用来构造因子
  Rot2 prior = Rot2::fromAngle(30 * degree);
  prior.print("goal angle");
  // 输入(维度，标准差)，得到噪声模型
  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
  // Symbol：用来表示待优化参数
  Symbol key('x',1);
  // 构造因子
  PriorFactor<Rot2> factor(key, prior, model);

2. 创建因子图，添加因子

在这个例子中，只有一个因子。

  NonlinearFactorGraph graph;
  graph.push_back(factor);
  graph.print("full graph");

3. 设置初始估计值

在优化前，需要给定待优化变量的初始值，在这个例子中，这个系统状态是“旋转值”结构，Values数据类型的用法类似与stl标准库的map类型。

所有的“旋转值”类型的接口是相同的，如果有多种类型的变量，它只取决于用于查找适当的值映射的键的类型

  // 创建Values
  Values initial;
  // insert(key,初始值)
  initial.insert(key, Rot2::fromAngle(20 * degree));
  initial.print("initial estimate");

4. 优化

  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("final result");

 

---

完整代码 

#include <gtsam/geometry/Rot2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>


using namespace std;
using namespace gtsam;

const double degree = M_PI / 180;

int main() {

  // 先验：测量值，用来构造因子
  Rot2 prior = Rot2::fromAngle(30 * degree);
  prior.print("goal angle");
  // 输入(维度，标准差)，得到噪声模型
  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
  // Symbol：用来表示待优化参数
  Symbol key('x',1);
  // 构造因子
  PriorFactor<Rot2> factor(key, prior, model);

  NonlinearFactorGraph graph;
  graph.push_back(factor);
  graph.print("full graph");

  // 创建Values
  Values initial;
  // insert(key,初始值)
  initial.insert(key, Rot2::fromAngle(20 * degree));
  initial.print("initial estimate");

  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("final result");

  return 0;
}

cmake_minimum_required(VERSION 2.8)

project(SimpleRotation)

# 寻找第三方库，使用大小写都可以，这里列举了两种方式
find_package(Boost COMPONENTS thread filesystem date_time system REQUIRED)
FIND_PACKAGE(GTSAM REQUIRED)

# 包含第三方库头文件路径，可以使用绝对路径
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${GTSAM_INCLUDE_DIR})
INCLUDE_DIRECTORIES("/usr/include/eigen3")

add_executable(${PROJECT_NAME} "SimpleRotation.cpp")
# 链接库
target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} -lgtsam -ltbb)
install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION bin)