GtSAM图优化库的使用

GtSAM图优化库的使用

最近定了自己的研究方向，是关于室内外移动机器人导航相关方面的研究，需要对GtSAM库进行研究。

安装

根据GtSAM的官方网站进行对给gtsam的安装。

git clone https://github.com/borglab/gtsam
cd gtsam
mkdir build
cd build
cmake ..
make check (optional, runs unit tests)
make install

初体验

仿照官网例程，进行编写

cmake_minimum_required(VERSION 2.8)
project(gtsam)

find_package(GTSAM REQUIRED QUIET)
include_directories(${GTSAM_INCLUDE_DIRS})
include_directories(
	${GTSAM_INCLUDE_DIR}
)
link_directories(
  	${GTSAM_LIBRARY_DIRS}
)
add_executable(use_gtsam use_gtsam.cpp)
target_link_libraries(use_gtsam gtsam)

这里可能会出现一个报错，显示找不到库，这里需要进入/usr/include/eigen3中的Eigen文件夹提取到/usr/include/中

#include<gtsam/base/Vector.h>
#include<gtsam/geometry/Pose2.h>
#include<gtsam/slam/BetweenFactor.h>
#include<gtsam/nonlinear/NonlinearFactorGraph.h>
#include<gtsam/nonlinear/Values.h>
#include<gtsam/nonlinear/Marginals.h>
#include<gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// #include<gtsam/nonlinear/DoglegOptimizer.h>
#include<gtsam/slam/PriorFactor.h>
using namespace gtsam;
using namespace std;
int main(int argc, char** argv)
{
    
    NonlinearFactorGraph graph;
    Pose2 priorMean(0.0, 0.0, 0.0);
    // noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
    auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));

    graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));

    Pose2 odometry(2.0, 0.0, 0.0);
    auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
    graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
    graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
    
    Values initial;
    initial.insert(1, Pose2(0.5, 0.0, 0.2));
    initial.insert(2, Pose2(2.3, 0.1, -0.2));
    initial.insert(3, Pose2(4.1, 0.1, 0.1));
    initial.print("\n Initial Estimate:\n");

    Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
    result.print("Final Result:\n");

    cout.precision(2);
    Marginals marginals(graph, result);
    cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
    cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
    cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;

    return 0;
}