程序员经验分享-hwhale

一起做激光SLAM[六]isam于SLAM位姿因子图优化的使用

2023-05-16

本节目标：学习gtsam与isam在二位位姿pose2和三维位姿pose3上的使用，并将isam用于位姿的因子图优化。

预期效果：将ICP匹配带来的瞬间位移变成对之前累积误差的消除。蓝色ICP无图优化，紫色ICP后进行图优化。

程序：https://gitee.com/eminbogen/one_liom

test_gtsam里有学习 gtsam，isam的四个程序

目录

图优化学习

图优化和因子图优化的简介

gtsam对于二维位姿的使用

isam对于二维位姿的使用

gtsam对于三维位姿的使用

isam对于三维位姿的使用

GPS因子加入

因子图优化用于SLAM

添加内容

示意

图优化学习

图优化和因子图优化的简介

干货：因子图优化的资源合集

深入理解图优化与g2o：图优化篇

iSAM2 笔记

简单来说就是图优化是一次获取所有位姿信息进行优化，因子图优化是逐帧获取逐帧优化，实时且只优化关联帧的位姿信息。

gtsam对于二维位姿的使用

 二维优化目标为优化所有x的位姿。

1.定义需要优化的图和优化的参数

NonlinearFactorGraph graph;
Values initials;

2.加入欲优化的初值，添加单点的先验因子，边因子

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));
graph.add(PriorFactor<Pose2>(Symbol('x', 1), Pose2(0, 0, 0), priorModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(5, 0, 0), odomModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(5, 0, -M_PI_2), odomModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 3), Symbol('x', 4), Pose2(5, 0, -M_PI_2), odomModel));
graph.add(BetweenFactor<Pose2>(Symbol('x', 4), Symbol('x', 5), Pose2(5, 0, -M_PI_2), odomModel));

 3.设置gtsam参数，并将参数输入gtsam优化函数

GaussNewtonParams parameters;
parameters.setVerbosity("ERROR");
parameters.setMaxIterations(20);
parameters.setLinearSolverType("MULTIFRONTAL_QR");

 4.进行图优化，获取优化结果

GaussNewtonOptimizer optimizer(graph, initials, parameters);
Values results = optimizer.optimize();

整体程序来源于官方文档：

#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
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<Pose2>(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<Pose2>(Symbol('x', 1), Symbol('x', 2), Pose2(5, 0, 0), odomModel));
    graph.add(BetweenFactor<Pose2>(Symbol('x', 2), Symbol('x', 3), Pose2(5, 0, -M_PI_2), odomModel));
    graph.add(BetweenFactor<Pose2>(Symbol('x', 3), Symbol('x', 4), Pose2(5, 0, -M_PI_2), odomModel));
    graph.add(BetweenFactor<Pose2>(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<Pose2>(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;
}

效果：

isam对于二维位姿的使用

1.定义需要优化的图和优化的参数

NonlinearFactorGraph graph;
Values initialEstimate;

2.加入欲优化的初值，添加单点的先验因子，边因子

initPose.push_back(Pose2(0.5, 0.0,  0.2   ));
initialEstimate.insert(i+1, initPose[i]);
......
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
......
gra.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
......

3.设置iSAM参数，并将参数输入ISAM优化函数

ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
ISAM2 isam(parameters);

 4.每次或多次加入后进行因子图优化

isam.update(graph, initialEstimate);
isam.update();

全部程序来源于博客《isam2 优化pose graph》

#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <vector>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>

using namespace std;
using namespace gtsam;

int main()
{

  //　向量保存好模拟的位姿和测量，到时候一个个往isam2里填加
  std::vector< BetweenFactor<Pose2> > gra;
  std::vector< Pose2 > initPose;

  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

  gra.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
  gra.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
  gra.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
  gra.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
  gra.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));

  initPose.push_back(Pose2(0.5, 0.0,  0.2   ));
  initPose.push_back( Pose2(2.3, 0.1, -0.2   ));
  initPose.push_back( Pose2(4.1, 0.1,  M_PI_2));
  initPose.push_back( Pose2(4.0, 2.0,  M_PI  ));
  initPose.push_back( Pose2(2.1, 2.1, -M_PI_2));

  ISAM2Params parameters;
  parameters.relinearizeThreshold = 0.01;
  parameters.relinearizeSkip = 1;
  ISAM2 isam(parameters);

  // 注意isam2的graph里只添加isam2更新状态以后新测量到的约束
  NonlinearFactorGraph graph;
  Values initialEstimate;

  // the first pose don't need to update
  for( int i =0; i<5 ;i++)
  {
          // Add an initial guess for the current pose
           initialEstimate.insert(i+1, initPose[i]);

           if(i == 0)
           {
               //  Add a prior on the first pose, setting it to the origin
               // A prior factor consists of a mean and a noise model (covariance matrix)
               noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
               graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));

           }else
           {

               graph.push_back(gra[i-1]);  // ie: when i = 1 , robot at pos2,  there is a edge gra[0] between pos1 and pos2
               if(i == 4)
               {
                   graph.push_back(gra[4]);  //  when robot at pos5, there two edge, one is pos4 ->pos5, another is pos5->pos2  (grad[4])
               }
               isam.update(graph, initialEstimate);
               isam.update();

               Values currentEstimate = isam.calculateEstimate();
               cout << "****************************************************" << endl;
               cout << "Frame " << i << ": " << endl;
               currentEstimate.print("Current estimate: ");

               // Clear the factor graph and values for the next iteration
               // 特别重要，update以后，清空原来的约束。已经加入到isam2的那些会用bayes tree保管，你不用操心了。
               graph.resize(0);
               initialEstimate.clear();
           }
  }
  return 0; 
}

效果就是逐帧的。

gtsam对于三维位姿的使用

对于三维位姿我们优化如下位姿数据的图。

与二维位姿不同，三维位姿要使用pose3，由欧拉角和t组成，只改变第二步即可。

1.定义需要优化的图和优化的参数

NonlinearFactorGraph graph;
Values initials;

2.加入欲优化的初值，添加单点的先验因子，边因子

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)));
graph.add(PriorFactor<Pose3>(Symbol('x', 1), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 0, 0)), priorNoise));
graph.add(BetweenFactor<Pose3>(Symbol('x', 1), Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, -1)), odometryNoise));
graph.add(BetweenFactor<Pose3>(Symbol('x', 2), Symbol('x', 3), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, 2)), odometryNoise));
graph.add(BetweenFactor<Pose3>(Symbol('x', 3), Symbol('x', 4), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 5, -1)), odometryNoise));
graph.add(BetweenFactor<Pose3>(Symbol('x', 4), Symbol('x', 5), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(-5, 0, -1)), odometryNoise));

 3.设置gtsam参数，并将参数输入gtsam优化函数

GaussNewtonParams parameters;
parameters.setVerbosity("ERROR");
parameters.setMaxIterations(20);
parameters.setLinearSolverType("MULTIFRONTAL_QR");

 4.进行图优化，获取优化结果

GaussNewtonOptimizer optimizer(graph, initials, parameters);
Values results = optimizer.optimize();

整体程序：

#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
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<Pose3>(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<Pose3>(Symbol('x', 1), Symbol('x', 2), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, -1)), odometryNoise));
    graph.add(BetweenFactor<Pose3>(Symbol('x', 2), Symbol('x', 3), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, 2)), odometryNoise));
    graph.add(BetweenFactor<Pose3>(Symbol('x', 3), Symbol('x', 4), Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 5, -1)), odometryNoise));
    graph.add(BetweenFactor<Pose3>(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<Pose3>(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;
}

效果有旋转矩阵和t,协方差为6*6的，应该是t的xyz和欧拉角rpy。

isam对于三维位姿的使用

同样只有第二步修改。

1.定义需要优化的图和优化的参数

NonlinearFactorGraph graph;
Values initialEstimate;

2.加入欲优化的初值，添加单点的先验因子，边因子

initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0.1, -0.1, 0)));
initialEstimate.insert(i+1, initPose[i]);
......
graph.push_back(PriorFactor<Pose3>(1, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 0, 0)), priorNoise));
......
gra.push_back(BetweenFactor<Pose3>(1, 2, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, -1)), model));
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
......

3.设置iSAM参数，并将参数输入ISAM优化函数

ISAM2Params parameters;
parameters.relinearizeThreshold = 0.01;
parameters.relinearizeSkip = 1;
ISAM2 isam(parameters);

 4.每次或多次加入后进行因子图优化

isam.update(graph, initialEstimate);
isam.update();

全部程序：

#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <vector>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>

using namespace std;
using namespace gtsam;

int main()
{

  //　向量保存好模拟的位姿和测量，到时候一个个往isam2里填加
  std::vector< BetweenFactor<Pose3> > gra;
  std::vector< Pose3 > initPose;

  // For simplicity, we will use the same noise model for odometry and loop closures
  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances(
          (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());

  gra.push_back(BetweenFactor<Pose3>(1, 2, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, -1)), model));
  gra.push_back(BetweenFactor<Pose3>(2, 3, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5, 0, 2)), model));
  gra.push_back(BetweenFactor<Pose3>(3, 4, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 5, -1)), model));
  gra.push_back(BetweenFactor<Pose3>(4, 5, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(-5, 0, -1)), model));
  gra.push_back(BetweenFactor<Pose3>(5, 2, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, -5, 0)), model));

  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0.1, -0.1, 0)));
  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(6, 0.1, -1.1)));
  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10.1, -0.1, 0.9)));
  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(10, 5, 0)));
  initPose.push_back( Pose3(Rot3::RzRyRx(0, 0, 0), Point3(5.1, 4.9, -0.9)));

  ISAM2Params parameters;
  parameters.relinearizeThreshold = 0.01;
  parameters.relinearizeSkip = 1;
  ISAM2 isam(parameters);

  NonlinearFactorGraph graph;
  Values initialEstimate;

  for( int i =0; i<5 ;i++)
  {
           initialEstimate.insert(i+1, initPose[i]);

           if(i == 0)
           {
               noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances(
		(Vector(6) << 1e-2, 1e-2, M_PI * M_PI, 1e8, 1e8, 1e8).finished()); 
               graph.push_back(PriorFactor<Pose3>(1, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 0, 0)), priorNoise));

           }else
           {

               graph.push_back(gra[i-1]);  // ie: when i = 1 , robot at pos2,  there is a edge gra[0] between pos1 and pos2
               if(i == 4)
               {
                   graph.push_back(gra[4]);  //  when robot at pos5, there two edge, one is pos4 ->pos5, another is pos5->pos2  (grad[4])
               }
               isam.update(graph, initialEstimate);
               isam.update();

               Values currentEstimate = isam.calculateEstimate();
               cout << "****************************************************" << endl;
               cout << "Frame " << i << ": " << endl;
               currentEstimate.print("Current estimate: ");

               // Clear the factor graph and values for the next iteration
               // 特别重要，update以后，清空原来的约束。已经加入到isam2的那些会用bayes tree保管，你不用操心了。
               graph.resize(0);
               initialEstimate.clear();
           }
  }
  return 0; 
}

效果：

GPS因子加入

gtsam::GPSFactor gps_factor(Ind, gtsam::Point3(gps_x, gps_y, gps_z), gps_noise);
graph.add(gps_factor);
isam.update(graph, initialEstimate);

因子图优化用于SLAM

添加内容

0.配置文件添加

CMakelist：
find_package(Boost COMPONENTS thread filesystem date_time system REQUIRED)
find_package(GTSAM REQUIRED)
include_directories(
	include
	${Boost_INCLUDE_DIR}
	${GTSAM_INCLUDE_DIR})
add_executable(map src/map.cpp)
target_link_libraries(map ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES} ${Boost_LIBRARIES} -lgtsam -ltbb)

package：
<build_depend>GTSAM</build_depend>

头文件：
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>

1.先验信息

//保存初始点信息，为先验因子。下面操作是把四元数转换为旋转矩阵，旋转矩阵变化为欧拉角
Eigen::Matrix3d rx = q_w_curr.toRotationMatrix();
Eigen::Vector3d ea = rx.eulerAngles(2,1,0);
initialEstimate.insert(temp_laserCloudMap_Ind+1,Pose3(Rot3::RzRyRx(ea[0], ea[1], ea[2]), Point3(t_w_curr.x(), t_w_curr.y(), t_w_curr.z())) );
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<Pose3>(1, Pose3(Rot3::RzRyRx(0, 0, 0), Point3(0, 0, 0)), priorNoise));

2.当前帧信息和里程计变换信息

//保存当前位姿和里程计变换的信息到gtsam的graph，
Eigen::Matrix3d rx_last = q_w_last.toRotationMatrix();
Eigen::Vector3d ea_last = rx_last.eulerAngles(2,1,0);
Eigen::Matrix3d rx_curr = q_w_curr.toRotationMatrix();
Eigen::Vector3d ea_curr = rx_curr.eulerAngles(2,1,0);
initialEstimate.insert(temp_laserCloudMap_Ind+1,Pose3(Rot3::RzRyRx(ea_curr[0], ea_curr[1], ea_curr[2]), Point3(t_w_curr.x(), t_w_curr.y(), t_w_curr.z())) );
//gtsam提供一种容易求两点之间位姿变化的函数between，用于里程计变化信息的录入
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(ea_last[0], ea_last[1], ea_last[2]), Point3(t_w_last.x(), t_w_last.y(), t_w_last.z()));
gtsam::Pose3 poseTo =   Pose3(Rot3::RzRyRx(ea_curr[0], ea_curr[1], ea_curr[2]), Point3(t_w_curr.x(), t_w_curr.y(), t_w_curr.z()));
noiseModel::Diagonal::shared_ptr odom_noise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
graph.add(BetweenFactor<Pose3>(temp_laserCloudMap_Ind,temp_laserCloudMap_Ind+1, poseFrom.between(poseTo),odom_noise));

3.闭环信息

//发生闭环，添加闭环信息到gtsam的graph，闭环noise使用icp得分
Values isamCurrentEstimate1;
Pose3 closeEstimate;
isamCurrentEstimate1 = isam.calculateEstimate();
closeEstimate = isamCurrentEstimate1.at<Pose3>(history_close_Ind+1);

Eigen::Matrix3d rx_temp = q_w_temp.toRotationMatrix();
Eigen::Vector3d ea_temp = rx_temp.eulerAngles(2,1,0);
gtsam::Pose3 poseTemp =   Pose3(Rot3::RzRyRx(ea_temp[0], ea_temp[1], ea_temp[2]), Point3(t_w_temp.x(), t_w_temp.y(), t_w_temp.z()));
noiseModel::Diagonal::shared_ptr closure_noise = noiseModel::Diagonal::Variances((Vector(6) << icp_score, icp_score, icp_score, icp_score, icp_score,icp_score).finished());
graph.add(BetweenFactor<Pose3>(history_close_Ind+1,temp_laserCloudMap_Ind+1, closeEstimate.between(poseTemp),closure_noise));

4.优化后路径输出

//输出gtsam优化后的轨迹
nav_msgs::Path laserGtsamPath;
for(int i=0;i<temp_laserCloudMap_Ind;i++)
{
    nav_msgs::Odometry laserGtsamOdometry;
    Pose3 oneEstimate; 
    oneEstimate = isamCurrentEstimate2.at<Pose3>(i+1);
    laserGtsamOdometry.header.frame_id = "map";
    laserGtsamOdometry.child_frame_id = "map_child";
    laserGtsamOdometry.header.stamp = all_time[i];
    laserGtsamOdometry.pose.pose.orientation.x = oneEstimate.rotation().toQuaternion().x();
    laserGtsamOdometry.pose.pose.orientation.y = oneEstimate.rotation().toQuaternion().y();
    laserGtsamOdometry.pose.pose.orientation.z = oneEstimate.rotation().toQuaternion().z();
    laserGtsamOdometry.pose.pose.orientation.w = oneEstimate.rotation().toQuaternion().w();
    laserGtsamOdometry.pose.pose.position.x = oneEstimate.translation().x();
    laserGtsamOdometry.pose.pose.position.y = oneEstimate.translation().y();
    laserGtsamOdometry.pose.pose.position.z = oneEstimate.translation().z();

    geometry_msgs::PoseStamped laserGtsamPose;
    laserGtsamPose.header = laserGtsamOdometry.header;
    laserGtsamPose.pose = laserGtsamOdometry.pose.pose;
    laserGtsamPath.header.stamp = laserGtsamOdometry.header.stamp;
    laserGtsamPath.poses.push_back(laserGtsamPose);
    laserGtsamPath.header.frame_id = "map";
    pubLaserGtsamPath.publish(laserGtsamPath); 
}

示意

可以看出对于原本位移明显地区有了很好的平滑，且修改是基于全局的累积误差消除进行的。

 

 

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

Slam

isam

一起做激光

位姿因子图优化的使用

一起做激光SLAM[六]isam于SLAM位姿因子图优化的使用 的相关文章

  • 大师兄！SLAM 为什么需要李群与李代数？

    from https mp weixin qq com s sVjy9kr 8qc9W9VN78JoDQ 作者 electech6 来源 计算机视觉life 编辑 Tony 很多刚刚接触SLAM的小伙伴在看到李群和李代数这部分的时候 都有点

  • SLAM笔记（四）运动恢复结构的几何数学(本征矩阵、单应矩阵、基础矩阵)

    1 间接法进行运动恢复的前提假设 对于结构与运动或视觉三维重建中 通常假设已经通过特征匹配等方法获取了匹配好的点对 先求出匹配点对再获取结构和运动信息的方法称作间接法 间接法最重要的三个假设是 1 拥有一系列两帧之间的匹配点对 但同时假设匹

  • 《视觉SLAM十四讲》第一版源码slambook编译调试

    slambook master ch2 编译正常 log如下 slambook master ch2 mkdir build cd build cmake make j8 The C compiler identification is G

  • 【大一立项】如何亲手搭建ROS小车：硬件和软件介绍

    本次博客将详细介绍上篇博客中提到的ROS小车的硬件和软件部分 由于十一实验室不开门 所以部分代码还没有上传到Github 下位机 下位机使用Arduino 因为大一上刚学完用Arduino做循迹小车 其实Arduino作为ROS小车的下位机

  • Sophus使用记录

    sophus库是一个基于Eigen的C 李群李代数库 可以用来方便地进行李群李代数的运算 头文件 主要用到以下两个头文件 include

  • Difference Between LiDAR and RADAR——LiDAR和RADAR的不同

    Difference Between LiDAR and RADAR 原文连接 https www differencebetween com difference between lidar and vs radar 翻译 RADAR和L

  • vscode配置eigen3

    目录 1 头文件包含 2 c cpp properties json 3 CMakeList txt 4 完整代码 1 头文件包含 Eigen 核心部分 include

  • 动态场景下基于实例分割的SLAM（毕业设计开题及语义分割部分）

    动态场景下基于实例分割的SLAM 毕业论文设计思路及流水 前言 今年选了个比较难的毕设题目 这里记录一下自己思路和流程 为之后的学弟学妹 划掉 铺个方向 会按日期不定期的更新 一 开题 2019 12 24 考研前选择课题是 利用深度学习对

  • PnP 问题

    欢迎访问我的博客首页 PnP 问题 1 DLT 2 P3P 3 G2O 求解 PnP 3 1 单目 3 2 双目 4 自定义顶点与边优化内参 4 1 二元边 4 2 三元边 4 3 总结 5 参考 PnP Perspective n Poi

  • SLAM-hector_slam 简介与使用

    hector slam功能包使用高斯牛顿方法 不需要里程计数据 只根据激光信息便可构建地图 所以他的总体框架如下 hector slam功能包 hector slam的核心节点是hector mapping 它订阅 scan 话题以获取SL

  • 1-如何安装ROS

    如何安装ROS 大家好 我是如何 今天尝试在Ubantu下安装ROS Robot Operating System 测试环境 虚拟机VMware Ubantu20 04 准备步骤 添加ROS软件源 sudo sh c echo deb ht

  • 快看！那个学vSLAM的上吊了！ —— （一）综述

    不同于之前发布的文章 我将使用一种全新的方式 iPad Notability Blog的方式打开这个板块的大门 原因有两个 1 Notability更方便手写长公式 也方便手绘坐标系变换等等 2 之前Apple Pencil找不到了新破费买

  • 舒尔补-边际概率-条件概率

    margin求边际概率的时候喜欢通过舒尔补的形式去操作信息矩阵 如p b c 求积分p a b c da 从上图可知 边缘概率直接看协方差矩阵比较方便 边际概率的方差就是取对应联合分布中相应的协方差块 信息矩阵是由舒尔补的形式计算 此形式也

  • 二.全局定位--开源定位框架livox-relocalization实录数据集测试

    相关博客 二十五 SLAM中Mapping和Localization区别和思考 goldqiu的博客 CSDN博客 二十五 SLAM中Mapping和Localization区别和思考 goldqiu的博客 CSDN博客 基于固态雷达的全局

  • Object SLAM: An Object SLAM Framework for Association, Mapping, and High-Level Tasks 论文解读

    是一篇来自机器人顶刊T RO的文章 发表于2023 5 An Object SLAM Framework for Association Mapping and High Level Tasks 论文 An Object SLAM Fram

  • Eigen几何模块的使用方法

    include

  • SLAM练习题（十一）—— G2O实战

    SLAM 学习笔记 写在前面的话 算是一点小小的感悟吧 估计位姿的方法有线性方法和非线性方法 线性方法就是特征点法中的2D 2D的对极约束 3D 2D的PnP问题 非线性方法有BA优化 它将位姿的估计问题转换成了一个误差关于优化量的最小二乘

  • 什么是深度学习的无监督学习与有监督学习

    无监督学习 深度学习中的无监督学习方法是一种训练算法 它在没有标注输出的情况下从输入数据中学习模式和特征 这种方法的核心是探索和理解数据的内在结构和分布 而不是通过已知的输出来指导学习过程 无监督学习在深度学习领域有许多不同的形式和应用 以

  • VB6 - Foxpro 2.6 XP 上没有 isam 错误

    老板给了我一些相当古老的遗留代码 全部用 VB6 完成 安装 Visual Studio 6 和其他一些东西后 我终于能够打开该项目并实际编译 运行它 我的新问题来自这行代码 Set db DBEngine Workspaces 0 Ope

  • 高翔博士Faster-LIO论文和算法解析

    说明 题目 Faster LIO 快速激光IMU里程计 参考链接 Faster LIO 快速激光IMU里程计 iVox Faster Lio 智行者高博团队开源的增量式稀疏体素结构 Faster Lio是高翔博士在Fast系列的新作 对标基

随机推荐

热门标签