使用 C++ 和 Eigen 库理解 IMU 数据处理与可视化

使用 C++ 和 Eigen 库理解 IMU 数据处理与可视化

        在本文中，我们将探讨如何使用 C++ 和 Eigen 库处理和可视化惯性测量单元（IMU）数据。IMU 数据在各种应用中至关重要，包括机器人技术、导航系统和虚拟现实。我们将探讨如何读取 IMU 数据，处理数据以估计姿态，并使用 Pangolin 可视化轨迹。

读取 IMU 数据

        我们首先从文件中读取 IMU 数据。数据包括时间戳、角速度和加速度。我们使用一个简单的函数 readIMUData 解析数据，并将其存储在 ImuFrame 结构的向量中。

IMU的数据格式

1403636579758555392 -0.099134701513277898 0.14730578886832138 0.02722713633111154 8.1476917083333333 -0.37592158333333331 -2.4026292499999999

估计姿态

        接下来，我们实现一个 IMU 跟踪器类（IMUTracker）来估计随时间变化的姿态（位置和方向）。我们用第一个 IMU 帧初始化跟踪器，并顺序处理后续帧来更新姿态估计。姿态估计的核心是将角速度积分以更新方向，将加速度积分以更新位置。

可视化

        一旦我们估计了姿态，我们就使用 Pangolin 在三维空间中可视化轨迹。我们创建一个窗口，并在其中渲染轨迹。每个姿态表示为一个点，我们连接连续的姿态以可视化轨迹。

代码展示

IMUTracker.h

// 包含必要的头文件
#pragma once

#include <eigen3/Eigen/Dense> // Eigen 线性代数库的头文件
#include <fstream>            // 文件输入输出流
#include <iomanip>            // 格式控制头文件，用于输出格式控制
#include <iostream>           // 标准输入输出流
#include <pangolin/pangolin.h> // Pangolin 可视化库头文件
#include <unistd.h>             // 提供对 POSIX 操作系统 API 的访问
#include <vector>               // 向量容器头文件

using namespace std;
using namespace Eigen; // Eigen 命名空间

// 姿态结构体
struct Pose
{
    uint64_t timestamp;       // 时间戳
    Vector3d position;        // 位置
    Quaterniond orientation;  // 方向
    Vector3d linear_vel;      // 线性速度
    Vector3d ang_vel;         // 角速度
};

// 点结构体
struct Point
{
    Vector3d pos;       // 位置
    Matrix3d orien;     // 方向
    Vector3d ang_vel;   // 角速度
    Vector3d linear_vel; // 线性速度
};

// IMU 数据帧结构体
struct ImuFrame
{
    uint64_t timestamp; // 时间戳
    Vector3d ang_vel;   // 角速度
    Vector3d acc_vel;   // 加速度
};

// 读取 IMU 数据的函数
bool readIMUData(const string &path, vector<ImuFrame> &imu_msg_buffer)
{
    ifstream fin; // 文件输入流对象
    fin.open(path, ios::in); // 打开文件
    if (!fin.is_open())
    {
        cout << "打开文件失败。" << endl; // 输出错误信息
        return false;
    }

    // 读取文件中的数据，并存储到 imu_msg_buffer 向量中
    for (int i = 0; i < 36820; i++)
    {
        double data[7]; // 存储数据的数组
        for (int j = 0; j < 7; j++)
        {
            fin >> data[j]; // 读取数据
        }

        ImuFrame IMUData; // 创建 ImuFrame 对象
        IMUData.timestamp = data[0]; // 设置时间戳
        IMUData.ang_vel = Vector3d(data[1], data[2], data[3]); // 设置角速度
        IMUData.acc_vel = Vector3d(data[4], data[5], data[6]); // 设置加速度

        imu_msg_buffer.push_back(IMUData); // 将数据添加到 imu_msg_buffer 中
    }

    return true; // 返回 true，表示读取成功
}

// 保存轨迹数据到 TUM 格式文件的函数
void saveTrajectoryTum(const string &filename, vector<Pose> &vPose)
{
    ofstream f; // 文件输出流对象
    f.open(filename.c_str()); // 打开文件
    f << fixed; // 设置输出格式

    // 将姿态数据写入文件
    for (auto iter = vPose.begin(); iter != vPose.end(); iter++)
    {
        Quaterniond q = (*iter).orientation;
        Vector3d t = (*iter).position;
        f << setprecision(6) << (*iter).timestamp << setprecision(7) << " " << t(0) << " " << t(1) << " " << t(2) << " "
          << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << endl;
    }
    f.close(); // 关闭文件
}

// 显示轨迹的函数
void showTrack(vector<Isometry3d, aligned_allocator<Isometry3d>> &poses)
{
    // 创建窗口并设置 OpenGL 渲染状态
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    // 设置 OpenGL 视图状态
    pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
                                      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));

    // 创建 Pangolin 显示器并设置其边界和处理程序
    pangolin::View &d_cam = pangolin::CreateDisplay()
                                .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
                                .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) // 循环直到用户关闭窗口
    {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); // 清除颜色缓冲区和深度缓冲区
        d_cam.Activate(s_cam); // 激活相机视图

        // 渲染轨迹
        for (auto &pose : poses)
        {
            glPointSize(1.0);
            glBegin(GL_POINTS);
            glColor3f(0.0, 1.0, 0.0);
            glVertex3d(pose(0, 2), pose(1, 2), pose(2, 2)); // 绘制点
            glEnd();
        }

        pangolin::FinishFrame(); // 完成当前帧的绘制
        usleep(5000); // 休眠 5 毫秒
    }
}

// IMU 跟踪器类
class IMUTracker
{
  public:
    IMUTracker() // 构造函数
    {
        Vector3d zero{0.0, 0.0, 0.0}; // 零向量
        point_.pos = zero; // 设置位置
        point_.orien = Matrix3d::Identity(); // 设置方向为单位矩阵
        point_.linear_vel = zero; // 设置线性速度
        point_.ang_vel = zero; // 设置角速度
        firstFrame_ = true; // 第一帧标志

        // 读取 IMU 数据到缓冲区
        if (!readIMUData("/Downloads/pythonRead/imu_data_0329.txt", imu_msg_buffer_))
        {
            cout << "读取 IMU 数据失败。" << endl;
        }

        // 创建初始姿态
        Pose p0;
        p0.timestamp = imu_msg_buffer_[0].timestamp;
        p0.position = Vector3d(0., 0., 0.);
        p0.orientation = Quaterniond(1., 0., 0., 0.);
        p0.linear_vel = Vector3d(0., 0., 0.);
        p0.ang_vel = Vector3d(0., 0., 0.);
        vp_.push_back(p0); // 将初始姿态添加到向量中
    }

    ~IMUTracker() // 析构函数
    {
    }

    void track() // 跟踪函数
    {
        for (auto &msg : imu_msg_buffer_)
        {
            if (firstFrame_) // 第一帧处理
            {
                prev_time_ = msg.timestamp;
                deltaT_ = 0;
                setGravity(msg.acc_vel); // 设置重力
                firstFrame_ = false;
            }
            else
            {
                deltaT_ = (msg.timestamp - prev_time_) * 1e-9;
                prev_time_ = msg.timestamp;
                calOrien(msg.ang_vel); // 计算方向
                calPos(msg.acc_vel); // 计算位置
                updatePos(point_); // 更新姿态
            }
        }
    }

    /**
     * @brief set the first imu frame's acc_vel as gravity
     *
     * @param msg
     */
    void setGravity(Vector3d &msg) // 设置重力函数
    {
        gravity_ = Vector3d(msg); // 设置重力向量
    }

    /**
     * @brief
     *
     * @param msg 加速度
     */
    void calPos(Vector3d &msg) // 计算位置函数
    {
        Vector3d acc_i(msg);
        Vector3d acc_w = point_.orien * acc_i;
        point_.linear_vel += deltaT_ * (acc_w - gravity_);
        point_.pos += deltaT_ * point_.linear_vel;
    }

    /**
     * @brief
     *
     * @param msg 角速度
     */
    void calOrien(Vector3d &msg) // 计算方向函数
    {
        point_.ang_vel = msg;
        Matrix3d B; // 角速度 * 时间 = 角度（表示为反对称矩阵）
        B << 0, -msg.z() * deltaT_, msg.y() * deltaT_, msg.z() * deltaT_, 0, -msg.x() * deltaT_, -msg.y() * deltaT_,
            msg.x() * deltaT_, 0;

        double sigma = sqrt(pow(msg.x(), 2) + pow(msg.y(), 2) + pow(msg.z(), 2)) * deltaT_;
        point_.orien *= (Matrix3d::Identity() + (sin(sigma) / sigma) * B - ((1 - cos(sigma)) / pow(sigma, 2)) * B * B);
    }

    /**
     * @brief current pose
     *
     * @param point
     */
    void updatePos(Point &point) // 更新姿态函数
    {
        Pose p;

        p.timestamp = prev_time_; // 设置时间戳

        p.position = Vector3d(point.pos); // 设置位置
        p.linear_vel = Vector3d(point.linear_vel); // 设置线性速度
        p.ang_vel = Vector3d(point.ang_vel); // 设置角速度

        // 从旋转矩阵中计算四元数
        p.orientation.x() = (point.orien(2, 1) - point.orien(1, 2)) / 4;
        p.orientation.y() = (point.orien(0, 2) - point.orien(2, 0)) / 4;
        p.orientation.z() = (point.orien(1, 0) - point.orien(0, 1)) / 4;
        p.orientation.w() = sqrt(1 + point.orien(0, 0) + point.orien(1, 1) + point.orien(2, 2)) / 2;

        vp_.push_back(p); // 将姿态添加到向量中
    }

  public:
    Point point_; // 点对象
    bool firstFrame_; // 是否是第一帧
    vector<ImuFrame> imu_msg_buffer_; // IMU 数据缓冲区

    Vector3d gravity_; // 重力向量
    double deltaT_; // 时间间隔
    uint64_t prev_time_; // 上一帧时间戳
    vector<Pose> vp_; // 姿态向量
};

test.cpp

#include "IMUTracker.h"
#include <iostream>
#include <vector>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv)
{
    IMUTracker imu_tracker;
    imu_tracker.track();
    // saveTrajectoryTum("../imu_traj.tum", imu_tracker.vp_);

    // vector<Pose> -> vector<Isometry3d>
    // Isometry3d: [R t]
    //             [0 1]
    vector<Isometry3d, aligned_allocator<Isometry3d>> poses;
    for (auto &p : imu_tracker.vp_)
    {
        Isometry3d T(Quaterniond(p.orientation.w(), p.orientation.x(), p.orientation.y(), p.orientation.z()));
        T.pretranslate(Vector3d(p.position));
        poses.push_back(T);
    }
    showTrack(poses);

    return 0;
}

CMakelists.txt

 

cmake_minimum_required(VERSION 2.8)
project(imu_tracker)

find_package("/usr/include/eigen3")
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

add_executable(imu_tracker test.cpp)
target_link_libraries(imu_tracker ${Pangolin_LIBRARIES})

结果展示