GTSAM 基本数据类型

一抹烟霞

已于 2023-06-21 17:37:26 修改

阅读量1.3k

点赞数 2

CC 4.0 BY-SA版权

分类专栏：激光Slam 文章标签：自动驾驶

于 2020-08-21 12:13:18 首次发布

本文链接：https://blog.csdn.net/qq_34213260/article/details/107799727

本文详细介绍了GTSAM（Georgia Tech Smoothing and Mapping）库中的基本运动状态类型，包括平移、旋转、位姿、向量和导航状态。此外，还涉及了噪声模型、IMU模型，如ConstantBias和PreintegratedImuMeasurements，并概述了图模型的推理算法ISAM2、因子图、值结构和优化方法。文章通过实例展示了如何使用GTSAM进行双目里程计的优化计算。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

一、基本运动状态类型

1.1 平移和点坐标

gtsam::Point3

常用构造方式

gtsam::Point3(x, y, z)

1.2 旋转

gtsam::Rot3

常用构造方式

// 欧拉角，注意虽然函数名是RzRyRx，但实际是xyz顺序的
// static Rot3::RzRyRx(double x, double y, double z)
gtsam::Rot3::RzRyRx(roll, pitch, yaw)

//四元数  w,x,y,z
// Rot3(const Quaternion &q)
gtsam::Rot3(1, 0, 0, 0)
// 或者 gtsam::Rot3::Quaternion(double w, double x, double y, double z)
gtsam::Rot3::Quaternion(1, 0, 0, 0)

//旋转矩阵  Rot3(const Matrix3 &R)
gtsam::Matrix33 R;
gtsam::Rot3(R)

1.3 位姿（旋转+平移）

gtsam::Pose3

常用构造方式

// 旋转+平移构造
gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(x,y,z));

// 变换矩阵构造
gtsam::Pose3(T)

1.4 向量Vector

(gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()
//或者
gtsam::Vector Vector3(3);
Vector3 << 1e-2, 1e-2, 1e-2;

1.5 导航状态（位姿+ 速度/向量）

gtsam::NavState

构造函数

gtsam::NavState State_;
State_ = gtsam::NavState(Pose_, Vel_);

二、噪声(方差)模型

2.1 对角噪声模型

gtsam::noiseModel::Diagonal

float noiseScore = 0.5;
gtsam::Vector Vector3(3);
Vector3 << noiseScore, noiseScore, noiseScore;
//	A diagonal noise model created by specifying a Vector of sigmas
gtsam::noiseModel::Diagonal::Sigmas(Vector3);
// 等价于 
gtsam::noiseModel::Isotropic::Sigma(3, 0.5);

//A diagonal noise model created by specifying a Vector of variances
gtsam::noiseModel::Diagonal::Variances(Vector3);

//A diagonal noise model created by specifying a Vector of precisions,
gtsam::noiseModel::Diagonal::Precisions(Vector3);

区别

#include <iostream>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/slam/PriorFactor.h>

using namespace std;

int main()
{
   
   
    float noiseScore = 0.5;
    gtsam::Vector Vector3(3);
    Vector3 << noiseScore, noiseScore, noiseScore;
//	A diagonal noise model created by specifying a Vector of sigmas
    gtsam::noiseModel::Diagonal::shared_ptr SigmaNoise;
    SigmaNoise=gtsam::noiseModel::Diagonal::Sigmas(Vector3);
    SigmaNoise->print("SigmaNoise");
    cout<<"******************************************"<<endl;
//A diagonal noise model created by specifying a Vector of variances
    gtsam::noiseModel::Diagonal::shared_ptr VariancesNoise;
    VariancesNoise=gtsam::noiseModel::Diagonal::Variances(Vector3);
    VariancesNoise->print("VariancesNoise");
    cout<<"******************************************"<<endl;
//A diagonal noise model created by specifying a Vector of precisions,
    gtsam::noiseModel::Diagonal::shared_ptr PrecisionsNoise;
    PrecisionsNoise=gtsam::noiseModel::Diagonal::Precisions(Vector3);
    PrecisionsNoise->print("PrecisionsNoise");

    gtsam::noiseModel::Diagonal::shared_ptr IsotropicNoise;
    IsotropicNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 0.5);
    IsotropicNoise->print("IsotropicNoise");
    return 0;
}

运行结果如下图：
在这里插入图片描述

三、IMU模型

3.1 ConstantBias

gtsam::imuBias::ConstantBias

//由加速度和角速度的xyz组成的6维向量
gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());


//结构如下
acc = [0, 0, 0]' gyro = [0, 0, 0]'

3.2 PreintegratedImuMeasurements

gtsam::PreintegratedImuMeasurements

gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;

boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0)