GTSAM 学习（一）
GTSAM（Georgia Tech Smoothing and Mapping）是基于因子图的C++库，它可以解决slam和sfm的问题，当然它也可以解决简单或者更加复杂的估计问题。
主要由以下三个基本组成部分：
因子图（factor graph）：它属于一个二分图，由因子和变量连接而成。
变量（variables）：估计问题中的未知随机变量。
因子（factors）：非线性因子表示变量之间的约束，在slam中，可能为landmark或者odometry的读数。
第一个简单的例子是单个因子、单个变量执行一个根据先验信息（类似ceres初值）来进行的角度旋转非线性优化。
#include
#include
#include
#include
#include
#include
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 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)以下是对二维进行的两个因子（位姿因子、回环因子）根据先验initials进行的优化。
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
using namespace gtsam;
int main(int argc, char** argv)
{
NonlinearFactorGraph graph;
Values initials;
initials.insert(Symbol('x', 1), Pose2(0.2, -0.3, 0.2));
initials.insert(Symbol('x', 2), Pose2(5.1, 0.3, -0.1));
initials.insert(Symbol('x', 3), Pose2(9.9, -0.1, -M_PI_2 - 0.2));
initials.insert(Symbol('x', 4), Pose2(10.2, -5.0, -M_PI + 0.1));
initials.insert(Symbol('x', 5), Pose2(5.1, -5.1, M_PI_2 - 0.1));
initials.print("\nInitial Values:\n");
//固定第一个顶点，在gtsam中相当于添加一个先验因子
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Sigmas(Vector3(1.0, 1.0, 0.1));
graph.add(PriorFactor(Symbol('x', 1), Pose2(0, 0, 0), priorModel));
//二元位姿因子
noiseModel::Diagonal::shared_ptr odomModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));
graph.add(BetweenFactor(Symbol('x', 1), Symbol('x', 2), Pose2(5, 0, 0), odomModel));
graph.add(BetweenFactor(Symbol('x', 2), Symbol('x', 3), Pose2(5, 0, -M_PI_2), odomModel));
graph.add(BetweenFactor(Symbol('x', 3), Symbol('x', 4), Pose2(5, 0, -M_PI_2), odomModel));
graph.add(BetweenFactor(Symbol('x', 4), Symbol('x', 5), Pose2(5, 0, -M_PI_2), odomModel));
//二元回环因子
noiseModel::Diagonal::shared_ptr loopModel = noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.1));
graph.add(BetweenFactor(Symbol('x', 5), Symbol('x', 2), Pose2(5, 0, -M_PI_2), loopModel));
graph.print("\nFactor Graph:\n");
GaussNewtonParams parameters;
parameters.setVerbosity("ERROR");
parameters.setMaxIterations(20);
parameters.setLinearSolverType("MULTIFRONTAL_QR");
GaussNewtonOptimizer optimizer(graph, initials, parameters);
Values results = optimizer.optimize();
results.print("Final Result:\n");
Marginals marginals(graph, results);
cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
cout << "x4 covariance:\n" << marginals.marginalCovariance(Symbol('x', 4)) << endl;
cout << "x5 covariance:\n" << marginals.marginalCovariance(Symbol('x', 5)) << endl;
return 0;
} 
如果三维位置要使用Rot3（欧拉角和位移）；
#include
#include
#include
#include
#include
#include
#include
#include
#include
using namespace std;
using namespace gtsam;
int main(int argc, char** argv)
{
NonlinearFactorGraph graph;
Values initials;
initials.insert(Symbol('x', 1), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0.1, -0.1, 0)));
initials.insert(Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(6, 0.1, -1.1)));
initials.insert(Symbol('x', 3), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10.1, -0.1, 0.9)));
initials.insert(Symbol('x', 4), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10, 5, 0)));
initials.insert(Symbol('x', 5), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5.1, 4.9, -0.9)));
initials.print("\nInitial Values:\n");
//固定第一个顶点，在gtsam中相当于添加一个先验因子
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished());
graph.add(PriorFactor(Symbol('x', 1), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 0, 0)), priorNoise));
//二元位姿因子
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
graph.add(BetweenFactor(Symbol('x', 1), Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, -1)), odometryNoise));
graph.add(BetweenFactor(Symbol('x', 2), Symbol('x', 3), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, 2)), odometryNoise));
graph.add(BetweenFactor(Symbol('x', 3), Symbol('x', 4), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 5, -1)), odometryNoise));
graph.add(BetweenFactor(Symbol('x', 4), Symbol('x', 5), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(-5, 0, -1)), odometryNoise));
//二元回环因子
noiseModel::Diagonal::shared_ptr loopModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
graph.add(BetweenFactor(Symbol('x', 5), Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, -5, 0)), loopModel));
graph.print("\nFactor Graph:\n");
GaussNewtonParams parameters;
parameters.setVerbosity("ERROR");
parameters.setMaxIterations(20);
parameters.setLinearSolverType("MULTIFRONTAL_QR");
GaussNewtonOptimizer optimizer(graph, initials, parameters);
Values results = optimizer.optimize();
results.print("Final Result:\n");
Marginals marginals(graph, results);
cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
cout << "x4 covariance:\n" << marginals.marginalCovariance(Symbol('x', 4)) << endl;
cout << "x5 covariance:\n" << marginals.marginalCovariance(Symbol('x', 5)) << endl;
return 0;
} 
参考： GTSAM学习：：Example（1）SimpleRotation_dieju8330的博客-CSDN博客_gtsam学习
SLAM本质剖析-GTSAM - 古月居