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