GTSAM学习::Example(1)SimpleRotation

5 篇文章 2 订阅
5 篇文章 6 订阅

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)

 

  • 1
    点赞
  • 12
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值