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)