IMU预积分

Related Reading:

• gtsam关于imu预积分实现探究
• GTSAM中imu预积分及其因子图优化过程

#include <gtsam/navigation/PreintegrationParams.h>  // 引入GTSAM中用于预积分参数的头文件
#include <gtsam/navigation/ImuFactor.h>  // 引入IMU因子的头文件
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>  // 引入Levenberg-Marquardt优化器，用于非线性优化
#include <gtsam/nonlinear/Values.h>  // 引入GTSAM中的Values类，用于存储优化结果
#include <gtsam/slam/PriorFactor.h>  // 引入GTSAM中的先验因子，用于定义先验约束
#include <gtsam/geometry/Pose3.h>  // 引入GTSAM中的Pose3类，表示3D位姿
#include <gtsam/inference/Symbol.h>  // 引入GTSAM中的符号类，便于标识变量
#include <gtsam/navigation/NavState.h>  // 引入GTSAM中的NavState类，表示导航状态（位姿+速度）
#include <gtsam/slam/BetweenFactor.h>  // 引入GTSAM中的BetweenFactor类，表示两个变量之间的约束
#include <iostream>  // 引入标准输入输出库
#include <fstream>  // 引入文件操作库
#include <sstream>  // 引入字符串流库，用于处理CSV数据
#include <vector>  // 引入向量容器库
#include <cmath>  // 引入数学库
#include <iomanip>  // 引入格式化输入输出库

using namespace gtsam;  // 使用GTSAM命名空间
using namespace std;  // 使用标准命名空间

int main() {
    // 加载来自CSV文件的AVP数据
    ifstream file("avp_data.csv");  // 打开CSV文件
    if (!file.is_open()) {  // 如果文件无法打开
        cerr << "Unable to open the file!" << endl;  // 输出错误信息
        return -1;  // 返回错误代码
    }

    // 定义用于存储数据的变量
    vector<Vector3> euler_angles;  // 存储欧拉角（滚转角、俯仰角、偏航角）
    vector<Vector3> velocities;    // 存储速度数据（X, Y, Z）
    vector<Vector3> positions;     // 存储位置数据（X, Y, Z）
    vector<double> time_intervals; // 存储时间间隔（假设这里时间间隔为常数0.1秒）

    string line;
    // 跳过CSV文件的表头（如果存在）
    getline(file, line);

    // 从CSV文件中读取数据
    while (getline(file, line)) {
        stringstream ss(line);  // 使用字符串流解析每一行
        string value;
        vector<double> row_data;
        while (getline(ss, value, ',')) {  // 按照逗号分隔读取每个值
            row_data.push_back(stod(value));  // 转换为数字并存储
        }

        // 假设CSV列的顺序为：roll, pitch, yaw, velocity_x, velocity_y, velocity_z, pos_x, pos_y, pos_z, time_interval
        euler_angles.emplace_back(row_data[0], row_data[1], row_data[2]);  // 存储欧拉角
        velocities.emplace_back(row_data[3], row_data[4], row_data[5]);    // 存储速度
        positions.emplace_back(row_data[6], row_data[7], row_data[8]);     // 存储位置
        time_intervals.emplace_back(row_data[9]); // 存储时间间隔
    }
    file.close();  // 关闭文件

    // 重力加速度和IMU参数
    Vector3 gravity(0, 0, -9.81);  // 定义重力加速度向量，z轴为-9.81 m/s²
    auto params = PreintegrationParams::MakeSharedU(-9.81);  // 创建IMU预积分参数，使用重力加速度
    params->accelerometerCovariance = I_3x3 * 0.01;  // 设置加速度计噪声协方差（0.01）
    params->gyroscopeCovariance = I_3x3 * 0.01;      // 设置陀螺仪噪声协方差（0.01）
    params->integrationCovariance = I_3x3 * 0.01;    // 设置积分噪声协方差（0.01）

    // IMU偏置
    gtsam::imuBias::ConstantBias prior_bias;  // 假设IMU初始偏置为零

    // 创建初始状态
    Pose3 prior_pose(Rot3::RzRyRx(euler_angles[0]), Point3(positions[0].x(), positions[0].y(), positions[0].z()));  // 初始位姿
    Vector3 prior_velocity = velocities[0];  // 初始速度
    NavState prior_state(prior_pose, prior_velocity);  // 初始化导航状态

    // 噪声模型
    auto pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.3, 0.3, 0.3).finished());  // 位姿噪声模型
    auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1);  // 速度噪声模型
    auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 0.1);  // 偏置噪声模型

    // 创建因子图和初始估计容器
    NonlinearFactorGraph graph;  // 创建非线性因子图
    Values initialEstimate;  // 创建初始估计容器

    // 添加先验因子（先验约束）
    graph.add(PriorFactor<Pose3>(Symbol('x', 0), prior_pose, pose_noise_model));  // 添加位姿先验
    graph.add(PriorFactor<Vector3>(Symbol('v', 0), prior_velocity, velocity_noise_model));  // 添加速度先验
    graph.add(PriorFactor<imuBias::ConstantBias>(Symbol('b', 0), prior_bias, bias_noise_model));  // 添加偏置先验

    // 添加初始状态估计
    initialEstimate.insert(Symbol('x', 0), prior_pose);  // 添加初始位姿
    initialEstimate.insert(Symbol('v', 0), prior_velocity);  // 添加初始速度
    initialEstimate.insert(Symbol('b', 0), prior_bias);  // 添加初始偏置

    // 创建IMU预积分对象
    PreintegratedImuMeasurements preintegrated(params, prior_bias);  // 创建IMU预积分对象

    // 打开输出CSV文件，保存结果
    ofstream output_file("pre_avp_data_2.csv");
    output_file << "roll,pitch,yaw,velocity_x,velocity_y,velocity_z,x,y,z,time_interval\n";  // 写入CSV表头

    // 遍历IMU数据并进行处理
    for (size_t i = 1; i < euler_angles.size(); ++i) {
        double dt = time_intervals[i];  // 获取当前时间间隔
        Vector3 measured_acc = velocities[i];   // 使用速度作为加速度计数据的代理
        Vector3 measured_omega = euler_angles[i];  // 使用欧拉角作为陀螺仪数据的代理

        // 对IMU数据进行积分
        preintegrated.integrateMeasurement(measured_acc, measured_omega, dt);

        // 每隔一段时间或者在关键帧处添加IMU因子
        if (i % 10 == 0) {
            // 每10个步骤添加一个因子
            Pose3 current_pose(Rot3::RzRyRx(euler_angles[i]), Point3(positions[i].x(), positions[i].y(), positions[i].z()));  // 当前位姿
            Vector3 current_velocity = velocities[i];  // 当前速度

            // 添加IMU因子
            ImuFactor imu_factor(Symbol('x', i - 10), Symbol('v', i - 10),
                Symbol('x', i), Symbol('v', i),
                Symbol('b', i - 10), preintegrated);
            graph.add(imu_factor);

            // 添加偏置演化作为之间的因子
            auto bias_between_noise = noiseModel::Isotropic::Sigma(6, 0.01);
            graph.add(BetweenFactor<imuBias::ConstantBias>(Symbol('b', i - 10), Symbol('b', i), imuBias::ConstantBias(), bias_between_noise));

            // 更新初始估计
            if (initialEstimate.exists(Symbol('x', i))) {
                initialEstimate.update(Symbol('x', i), current_pose);
            }
            else {
                initialEstimate.insert(Symbol('x', i), current_pose);
            }
            if (initialEstimate

.exists(Symbol('v', i))) {
                initialEstimate.update(Symbol('v', i), current_velocity);
            }
            else {
                initialEstimate.insert(Symbol('v', i), current_velocity);
            }
            if (initialEstimate.exists(Symbol('b', i))) {
                initialEstimate.update(Symbol('b', i), prior_bias);
            }
            else {
                initialEstimate.insert(Symbol('b', i), prior_bias);
            }
        }
    }

    // 使用Levenberg-Marquardt进行批量优化
    LevenbergMarquardtParams lm_params;  // 创建Levenberg-Marquardt优化器的参数
    lm_params.setVerbosity("ERROR");  // 设置输出级别为“错误”以减少输出信息
    lm_params.setMaxIterations(100);   // 设置最大迭代次数为100
    lm_params.setRelativeErrorTol(1e-5); // 设置相对误差容忍度为1e-5
    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, lm_params);  // 创建优化器
    Values result = optimizer.optimize();  // 执行优化

    // 输出最终优化的状态并保存到CSV文件
    ofstream log_file("trajectory_log.txt");  // 创建日志文件保存轨迹数据
    for (size_t i = 0; i < euler_angles.size(); i += 10) {
        try {
            Pose3 pose = result.at<Pose3>(Symbol('x', i));  // 获取优化后的位姿
            Vector3 velocity = result.at<Vector3>(Symbol('v', i));  // 获取优化后的速度
            Vector3 rpy = pose.rotation().rpy();  // 获取优化后的欧拉角（滚转、俯仰、偏航）
            double time_interval = time_intervals[i];  // 获取时间间隔
            log_file << "Pose at step " << i << ": " << pose << endl;  // 写入日志文件
            log_file << "Velocity at step " << i << ": " << velocity.transpose() << endl;  // 写入日志文件
            output_file << fixed << setprecision(15) << rpy.x() << "," << rpy.y() << "," << rpy.z() << ","
                << velocity.x() << "," << velocity.y() << "," << velocity.z() << ","
                << pose.x() << "," << pose.y() << "," << pose.z() << ","
                << time_interval << "\n";  // 保存轨迹数据到CSV文件
        }
        catch (const exception& e) {
            cerr << "Exception while accessing data at step " << i << ": " << e.what() << endl;  // 如果访问数据时发生异常，输出错误信息
        }
    }

    // 关闭输出文件
    output_file.close();
    log_file.close();

    return 0;  // 程序正常结束
}

1. 数据读取：从CSV文件中加载AVP数据，具体包括欧拉角、速度、位置和时间间隔。
2. 预积分与IMU因子：使用IMU数据进行预积分并创建IMU因子，这在因子图优化中非常重要，尤其是在处理时间序列数据时。
3. 因子图优化：利用Levenberg-Marquardt优化器执行非线性优化，优化位姿、速度和IMU偏置。
4. 结果保存：最终的优化结果被输出到日志文件和CSV文件，包含了优化后的轨迹信息。

---

这段代码的主要功能是使用IMU（惯性测量单元）数据，结合图优化技术（尤其是 GTSAM 库），来进行 机器人或设备的状态估计，包括位姿（位置和朝向）、速度以及IMU的偏置估计。代码的核心任务是利用IMU的数据进行 预积分（Preintegration），构建一个因子图（Factor Graph），并通过 图优化 来估计系统的状态。

我们可以逐步分析这段代码的工作流程，以便理解它的具体功能。

---

背景知识：

在 机器人学 或 传感器融合 中，IMU 是一种常见的传感器，能够测量加速度和角速度。常见的应用包括 定位、姿态估计、惯性导航系统（INS） 等。

IMU 提供的是积分数据，意味着我们通常得到的是增量变化，而不是绝对位置、速度或姿态。因此，IMU 的数据需要通过 预积分（Preintegration） 和 优化算法 来推算出更准确的全局状态。

GTSAM 是一个处理 图优化 的库，常用于 SLAM（Simultaneous Localization and Mapping）和其他优化问题。通过构建一个 因子图（Factor Graph），它能够将各种约束（例如，来自传感器的测量数据）合并到一起，并通过优化算法（如 Levenberg-Marquardt）求解出最优的状态估计。

---

代码功能概述：

1. 读取IMU和位置数据：从CSV文件中读取IMU的角速度、加速度、位置、速度以及时间戳等信息。
2. 设置IMU预积分参数：使用IMU数据进行预积分，即通过加速度和角速度的增量计算设备的位姿和速度的变化。
3. 建立因子图：将IMU的预积分数据、设备的初始状态（位姿、速度、偏置）和噪声模型等添加到因子图中，构建一个非线性优化问题。
4. 执行图优化：使用 Levenberg-Marquardt优化器 对因子图进行优化，得到设备的最优状态（位姿、速度和偏置）。
5. 输出结果：将优化后的位姿、速度和其他信息保存到文件中，供后续分析使用。

---

具体步骤分析：

1. 数据读取（读取CSV文件中的IMU数据）

ifstream file("avp_data.csv");
if (!file.is_open()) {
    cerr << "Unable to open the file!" << endl;
    return -1;
}

首先，代码尝试从 "avp_data.csv" 文件中读取数据。如果文件打开失败，程序会输出错误信息并终止。

vector<Vector3> euler_angles;
vector<Vector3> velocities;
vector<Vector3> positions;
vector<double> time_intervals;

定义几个 vector 用来存储从CSV文件中读取的数据：

• euler_angles：每一行数据中的欧拉角（roll, pitch, yaw），表示物体的姿态。
• velocities：每一行数据中的速度（x, y, z）。
• positions：每一行数据中的位置（x, y, z）。
• time_intervals：时间间隔，假设为常量。

while (getline(file, line)) {
    stringstream ss(line);
    string value;
    vector<double> row_data;
    while (getline(ss, value, ',')) {
        row_data.push_back(stod(value));
    }

    // Assuming columns are ordered as: roll, pitch, yaw, velocity_x, velocity_y, velocity_z, pos_x, pos_y, pos_z, time_interval
    euler_angles.emplace_back(row_data[0], row_data[1], row_data[2]);
    velocities.emplace_back(row_data[3], row_data[4], row_data[5]);
    positions.emplace_back(row_data[6], row_data[7], row_data[8]);
    time_intervals.emplace_back(row_data[9]);
}
file.close();

这里逐行读取CSV文件中的数据，并将每行数据转换成对应的 欧拉角、速度、位置 和 时间间隔，存入前面定义的 vector 中。CSV文件的每一行数据的顺序假设是：欧拉角（roll, pitch, yaw）、速度（velocity_x, velocity_y, velocity_z）、位置（pos_x, pos_y, pos_z）和 时间间隔。

2. IMU预积分参数设置

Vector3 gravity(0, 0, -9.81);  // 设置重力加速度
auto params = PreintegrationParams::MakeSharedU(-9.81);  // 使用重力加速度创建IMU预积分参数
params->accelerometerCovariance = I_3x3 * 0.01;  // 设置加速度计噪声协方差
params->gyroscopeCovariance = I_3x3 * 0.01;      // 设置陀螺仪噪声协方差
params->integrationCovariance = I_3x3 * 0.01;    // 设置积分噪声协方差

在这部分代码中，我们创建了一个 IMU预积分参数对象 params。这个对象将用于对IMU数据进行预积分，计算设备的状态（位姿和速度）的增量。

• gravity 设置了重力加速度，指向 z轴负方向。
• params 对象初始化时使用了重力加速度，这对于IMU数据的处理至关重要。
• 通过调整 加速度计、陀螺仪 和 积分 的协方差矩阵，我们能够控制优化的精度和噪声。

3. 初始估计和噪声模型设置

Pose3 prior_pose(Rot3::RzRyRx(euler_angles[0]), Point3(positions[0].x(), positions[0].y(), positions[0].z()));
Vector3 prior_velocity = velocities[0];
NavState prior_state(prior_pose, prior_velocity);

• prior_pose：使用从CSV文件读取的第一个欧拉角和位置数据来设置设备的初始位姿。
• prior_velocity：使用从CSV文件读取的第一个速度数据来设置设备的初始速度。
• prior_state：封装位姿和速度信息，表示设备的初始状态。

auto pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.1, 0.1, 0.1, 0.3, 0.3, 0.3).finished());  // 位姿噪声模型
auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1);  // 速度噪声模型
auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 0.1);  // 偏置噪声模型

• pose_noise_model：为位姿（位置和朝向）设置噪声模型。
• velocity_noise_model：为速度设置噪声模型。
• bias_noise_model：为IMU的偏置设置噪声模型。

4. 因子图（Factor Graph）创建与初始化估计

NonlinearFactorGraph graph;  // 创建因子图
Values initialEstimate;  // 创建初始估计容器

• graph：用于存储因子图中的所有因子（约束）。
• initialEstimate：存储所有优化变量的初始值。

graph.add(PriorFactor<Pose3>(Symbol('x', 0), prior_pose, pose_noise_model));
graph.add(PriorFactor<Vector3>(Symbol('v', 0), prior_velocity, velocity_noise_model));
graph.add(PriorFactor<imuBias::ConstantBias>(Symbol('b', 0), prior_bias, bias_noise_model));

这里，我们为 位姿、速度 和 IMU偏置 添加了先验因子（表示我们对初始状态的知识和不确定性）。

initialEstimate.insert(Symbol('x', 0), prior_pose);  
initialEstimate.insert(Symbol('v', 0), prior_velocity);  
initialEstimate.insert(Symbol('b', 0), prior_bias);  

这些代码为初始估计容器插入了初始的位姿、速度和偏置估计值。

5. IMU数据预积分与因子添加

在每次读取IMU数据时，使用预积分技术计算设备状态的增量，并将其添加到因子图中。

PreintegratedImuMeasurements preintegrated(params, prior_bias);  // 创建IMU预积分对象

preintegrated 是一个 IMU预积分对象，用于根据每个时间步的加

速度和角速度来计算状态的增量。

for (size_t i = 1; i < euler_angles.size(); ++i) {
    double dt = time_intervals[i];  // 获取时间间隔
    Vector3 measured_acc = velocities[i];   // 假设速度作为加速度的代理
    Vector3 measured_omega = euler_angles[i];  // 假设欧拉角作为角速度的代理
    preintegrated.integrateMeasurement(measured_acc, measured_omega, dt);  // 进行IMU预积分
}

每处理一个数据点，程序就将加速度和角速度（这里简化为速度和欧拉角）通过 preintegrated 进行预积分，从而更新当前的设备状态。

---

总结

这段代码实现了基于 IMU数据 和 图优化 的 状态估计。具体来说，它从CSV文件中读取设备的IMU数据（加速度、角速度、位置、速度等），并通过 预积分 和 图优化 技术来计算设备的最优状态（位置、速度和偏置）。优化过程使用 Levenberg-Marquardt算法 来最小化因子图中的误差，从而得到更精确的估计结果。

最终，优化后的结果被保存到文件中，供后续分析使用。

Reference:
因子图优化及GTSAM中IMU预积分接口
VINS-Mono+Fusion源码解析系列（八）：IMU预积分的代码实现