帮我写一段slam中imu直接中值积分的c++代码

在SLAM（Simultaneous Localization and Mapping）中，惯性测量单元（IMU）的数据通常需要通过中值滤波来进行集成，以减小噪声并提高数据的可信度。以下是一个简单的C++代码片段，使用了Kalman滤波器中的中值滤波概念，但这并不是真正的IMU直接融合：

#include <Eigen/Dense>
#include <algorithm>

// 假设imu_data包含三轴加速度和角速度
struct ImuData {
    Eigen::Vector3f acceleration;
    Eigen::Vector3f angular_velocity;
};

// 中值滤波函数
ImuData median_filter(const std::vector<ImuData>& imu_history, size_t window_size) {
    if (imu_history.size() < window_size)
        return imu_history.back();

    Eigen::VectorXd data(window_size * 6);
    for (size_t i = 0; i < window_size; ++i) {
        data.segment(3 * i, 3) = imu_history[i].acceleration;
        data.segment(3 * i + 3, 3) = imu_history[i].angular_velocity;
    }

    // 对每3个连续数据点进行中值计算
    ImuData filtered_data = {Eigen::Vector3f(), Eigen::Vector3f()};
    for (int i = 0; i < 6; ++i) {
        std::nth_element(data.data() + i, data.data() + i + window_size / 2, data.data() + i + 3 * window_size);
        filtered_data.acceleration(i) = data(i + window_size / 2);
        filtered_data.angular_velocity(i) = data(i + 3 * window_size / 2);
    }
    return filtered_data;
}