使用 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})