GTSAM 基本数据类型

一、基本运动状态类型

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).finished