Gtsam学习笔记

Gtsam学习笔记

cmake引入

# 寻找第三方库，使用大小写都可以，这里列举了两种方式
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(gtsam_test main.cpp)
# 链接库 
target_link_libraries(gtsam_test ${Boost_LIBRARIES} -lgtsam -ltbb)
install(TARGETS gtsam_test RUNTIME DESTINATION bin)

clion没法识别的话就重启一下，可能是有的玩意没刷新出来。

因子factor

预定义的factor

头文件
#include  //二元因子，位姿之间，回环之间
#include  //一元因子，系统先验
//定义因子图
NonlinearFactorGraph graph;
//噪声定义 对角线矩阵：
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
//在因子图中加入一个因子
//   二元因子
graph.emplace_shared >(1, 2, Pose2(2, 0, 0     ), model);
			//参数解释 ^ ：因子类型  ^边		key 1、2  ^ 边的值			^ 噪声模型
//   一元因子
graph.emplace_shared >(1, Pose2(0, 0, 0), priorNoise);

生成factor

  Rot2 prior = Rot2::fromAngle(30 * degree);
  prior.print("goal angle"); 
  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
  Symbol key('x',1); // 一个key就是一个label
  PriorFactor factor(key, prior, model);

初值定义

一个图中要给每一个变量赋予一个初始值。

  Values initialEstimate; //定义初始值容器
  initialEstimate.insert(1, Pose2(0.5, 0.0,  0.2   )); //加入一个变量 arg1：变量的标签 arg2:这个变量的值

噪声定义

gtsam中的噪声有很多类型

namespace NM = gtsam::noiseModel;

// Set Noise parameters
Vector priorSigmas = Vector3(1,1,M_PI);
Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
const NM::Base::shared_ptr // 基类型
  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior 对角线噪声
  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust 各项同性噪声
  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust 

优化方法

GaussNewton法

引入 #include

GaussNewtonParams parameters;//参数对象
parameters.relativeErrorTol = 1e-5; // 迭代变化量小于这个值就退出
parameters.maxIterations = 100; //最大迭代次数
//创建一个高斯牛顿优化器 arg：		因子图 初值				参数对象
GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
//求出优化解 得到变量的最优值
Values result = optimizer.optimize();

LevenbergMarquardt法

引入 #include

LevenbergMarquardtOptimizer optimizer(graph,initialEstimate);
Values result=optimizer.optimize(); 

边缘化 marginal

    Marginals marginals(graph,result);
    for (int j = 1; j < 4; ++j) {
        boost::format fmt("\n x%1% covariance: \n %2% \n");
        fmt%j%marginals.marginalCovariance(j); //得到每个变量的协方差
        cout<

读写 g2o文件

#include  //引入头文件
//读
NonlinearFactorGraph::shared_ptr graph1;
Values::shared_ptr initialEstamate1;
string g2ofile=findExampleDataFile("noisyToyGraph.txt");
bool is3D =false;
boost::tie(graph1,initialEstamate1)=readG2o(g2ofile,is3D);
//写 g2o文件
writeG2o(graph,result,"graph2g2o.g2o"); //args: 因子图 ，变量结果，目标文件名

iSAM 更新过程

在文件ISAM2.cpp的update函数中

1. 添加新的因子 Impl::AddFactorsStep1
   $$Factors:=FactorFacto{r}^{\prime }$$

   //关键变量
   ISAM2Result result;
   isam.update(graph, initialEstimate);
   ISAM2Params params_; //在定义ISAM2实例的时候存储参数的。
   	//  parameters.relinearizeThreshold = 0.01;
     	// parameters.relinearizeSkip = 1;
   Impl //Internal implementation functions 内部实施函数
   useUnusedSlots //slot 插槽

2. 初始化新的变量 Impl::AddVariables
   $$\Theta :=\Theta \cup {\Theta }_{new}.$$

3. 标记线性化更新

4. 标记 变化量大于阈值的key
   $$\beta :J=\{{\Delta }_j\in \Delta |{\Delta }_j\ge \beta \}.$$

5. 标记所有与变量相关小团体clique 以及他们的所有父节点

6. 更新线性化点对于标记的变量
   $${\Theta }_J:={\Theta }_J+{\Delta }_J.$$

7. 线性化 新的因子

8. 重新整理贝叶斯数的树顶

LevenbergMarquardtOptimizer

LevenbergMarquardtOptimizer继承自 NonlinearOptimizer的optimize->defaultOptimize iterate();(这个函数是在子类中实现的，不是在父类中实现的)

Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
virtual const Values& optimize() { defaultOptimize(); return values(); }

在iterate中：
KaTeX parse error: Undefined control sequence: \f at position 2: \̲f̲ ̲Ax-b \approx h(…

1、
GaussianFactorGraph::shared_ptr linear = linearize();
就是对每一个因子进行线性化：NonlinearFactor：：linearize（）
2、
whitenedError 白化后的噪声