Boost库中Math 模块的插值（interpolation使用和示例

是 否 +2 论坛币

k人 参与回答

经管之家送您一份

应届毕业生专属福利!

求职就业群

赵安豆老师微信：zhaoandou666

经管之家联合CDA

送您一个全额奖学金名额~ !

立即领取

感谢您参与论坛问题回答

经管之家送您两个论坛币！

+2 论坛币

1. Boost.Math 插值模块功能概述

Boost.Math 库中包含一个专门用于数值插值与函数逼近的模块，具备高精度和良好的数值稳定性。该模块位于：

#include <boost/math/interpolators/...>

其主要支持以下几类插值方法：

插值方式	类名	特点说明
分段线性插值	barycentric_rational 、 piecewise_linear	实现简单，计算速度快
Cubic Hermite 三次样条	cardinal_cubic_b_spline 、 cubic_hermite	可控制导数行为，适用于连续运动轨迹建模
单调三次样条	pchip	无过冲振荡，适合处理物理测量数据
Akima 样条	akima	对异常值鲁棒性强，输出平滑
Barycentric 有理插值	barycentric_rational	精度高，适用于规律性强的数据集
B-Spline	cardinal_quadratic_b_spline 等	具有良好的连续性和平滑性表现

2. 工程实践中常用的四大插值器

在实际应用中，以下四种插值方法被广泛使用于机器人、SLAM、传感器融合等领域。

2.1 分段线性插值

boost::math::interpolators::piecewise_linear

适用场景：

• 需要快速完成基础插值任务
• 控制流程中的平滑处理
• SLAM 中对激光雷达点云进行时间维度上的补偿（扫描去畸变）

核心优势：

• 查询复杂度为 O(1)，响应迅速
• 无需额外计算导数信息
• 天然保持数据单调性

2.2 单调三次样条（PCHIP）

pchip

适用场景：

• 处理带有物理意义的数据序列（如位移、速度、姿态角）
• 激光惯性里程计（LIO）中对旋转或位置变量进行插值
• 避免因插值产生不真实的 overshoot 振荡

特性说明：

• 在单调区间内不会引入振荡
• 相比普通三次样条更加稳定可靠

2.3 Akima 样条插值

akima

适用场景：

• 含噪声的传感器测量序列处理
• 多传感器时间同步（例如 GPS、IMU、编码器 odom 数据对齐）
• 轨迹后处理中的平滑优化

突出优点：

• 对离群点或跳变不敏感
• 局部调整能力强，不影响整体趋势

2.4 Barycentric 有理插值

barycentric_rational

适用场景：

• 高精度科学计算需求
• 已知采样点数量较少但要求重建精度高

3. 各类插值方法的具体使用详解

3.1 分段线性插值（piecewise_linear）

#include <boost/math/interpolators/piecewise_linear.hpp>
#include <vector>
#include <iostream>

int main() {
    std::vector<double> xs = {0, 1, 2, 3};
    std::vector<double> ys = {0, 1, 4, 9};
    auto interp = boost::math::interpolators::piecewise_linear(xs, ys);
    std::cout << interp(1.5) << std::endl;  // 输出结果：2.5
}
    

优势总结：

• 执行效率极高
• 不会出现样条常见的过冲振荡现象
• 特别适用于 SLAM 前端中对激光点进行时间戳校正

3.2 单调三次样条 PCHIP 插值

#include <boost/math/interpolators/pchip.hpp>

int main() {
    std::vector<double> xs = {0, 1, 2, 3};
    std::vector<double> ys = {0, 1, 4, 9};
    auto interp = boost::math::interpolators::pchip(xs, ys);
    std::cout << interp(1.5) << std::endl;
}
    

关键特性：

• 若输入数据呈单调变化，则插值结果也将保持单调
• 不会产生 overshoot 现象
• 相较于标准 cubic spline 更适合处理物理量序列（如速度、姿态等）

在 SLAM 中的应用包括：

• IMU 数据的速度插值
• 激光雷达线束的时间去畸变处理（确保时间戳单调递增）

3.3 Akima 插值方法

#include <boost/math/interpolators/akima.hpp>

int main() {
    std::vector<double> xs = {0,1,2,3};
    std::vector<double> ys = {0,2,1,3};  // 存在突变或波动
    auto interp = boost::math::interpolators::akima(xs, ys);
    std::cout << interp(1.5) << std::endl;
}
    

主要优点：

• 具备较强的局部鲁棒性，对尖峰信号不敏感
• 生成的曲线 C1 连续，轨迹连贯平滑

典型应用场景：

• IMU 与 LiDAR 融合时的数据外推
• 机械臂运动轨迹的平滑处理
• 地图构建过程中轨迹的后期优化

3.4 Barycentric Rational 有理插值

#include <boost/math/interpolators/barycentric_rational.hpp>

int main() {
    std::vector<double> xs = {0,1,2,3};
    std::vector<double> ys = {1,2,0,4};

此插值方法通过有理函数形式实现更高阶逼近能力，在少量节点下仍能保持优异精度，常用于对误差容忍度极低的科学仿真或工程建模任务。

4. 插值器的导数计算功能

大多数插值方法均支持导数计算，可用于需要梯度信息的应用场景。常见接口包括：
interp.prime(x)        // 计算一阶导数
interp.double_prime(x)  // 计算二阶导数

这些导数功能广泛应用于以下领域：
- 优化算法中的梯度求解
- ICP（Iterative Closest Point）过程中的线性化处理
- 构建时间连续轨迹，如在连续时间SLAM中估计运动状态

5. SLAM系统中常用插值器及其适用场景

根据不同任务需求，选择合适的插值策略可显著提升系统性能：

| 应用场景 | 推荐插值方式 |
|--------|-------------|
| 激光雷达扫描去畸变（scan deskew） | piecewise_linear 或 pchip |
| IMU姿态插值（SO(3)空间） | pchip（适用于角度数据） |
| IMU位置与速度的连续逼近 | akima |
| 回环检测前对轨迹进行平滑处理 | akima |
| 多传感器间的时间同步 | piecewise_linear |
| 后端优化阶段的初始轨迹生成 | pchip 或 akima |

6. Boost库中各类插值方法的性能与复杂度对比

| 插值方式       | 构建复杂度 | 查询复杂度 | 总体特性说明         |
|--------------|----------|----------|--------------------|
| 分段线性（piecewise_linear） | O(N)     | O(1)     | 性能最优，响应最快       |
| PCHIP          | O(N)     | O(1)     | 稳定性强，输出质量高     |
| Akima          | O(N)     | O(1)     | 相较cubic spline更稳定 |
| Barycentric    | O(N)    | O(N)     | 高精度场景专用         |

7. 实际应用示例

1）连续时间轨迹类模板设计（支持位置与速度插值）

#pragma once
#include <vector>
#include <boost/math/interpolators/pchip.hpp>
#include <Eigen/Dense>

class ContinuousTrajectory {
public:
    using Vector3d = Eigen::Vector3d;

    // 根据时间戳和对应的位置序列构建轨迹
    ContinuousTrajectory(const std::vector<double>& timestamps,
                         const std::vector<Vector3d>& positions)
        : times_(timestamps), positions_(positions)
    {
        std::vector<double> x, y, z;
        for (const auto& p : positions) {
            x.push_back(p.x());
            y.push_back(p.y());
            z.push_back(p.z());
        }
        interp_x_ = boost::math::interpolators::pchip(x, times_);
        interp_y_ = boost::math::interpolators::pchip(y, times_);
        interp_z_ = boost::math::interpolators::pchip(z, times_);
    }

    // 获取指定时刻的位置
    Vector3d getPosition(double t) const {
        return Vector3d(interp_x_(t), interp_y_(t), interp_z_(t));
    }

    // 获取指定时刻的速度（即各维度的一阶导）
    Vector3d getVelocity(double t) const {
        return Vector3d(interp_x_.prime(t), interp_y_.prime(t), interp_z_.prime(t));
    }

private:
    std::vector<double> times_;
    std::vector<Vector3d> positions_;
    boost::math::interpolators::pchip interp_x_;
    boost::math::interpolators::pchip interp_y_;
    boost::math::interpolators::pchip interp_z_;
};

pchip

特点说明：
- 使用PCHIP插值确保轨迹单调且平滑，避免过冲。
- 可直接通过导数接口获取速度信息，适用于连续时间SLAM或IMU前向传播等任务。
- 结构清晰，易于扩展以支持加速度（二阶导）插值。

2）SO(3)/SE(3)空间下的插值模板（结合Boost与Sophus库）

#pragma once
#include <vector>
#include <boost/math/interpolators/pchip.hpp>
#include <Eigen/Dense>
#include <sophus/so3.hpp>

#include <sophus/se3.hpp>

class SE3Trajectory {
private:
    // 时间戳与位姿数据
    std::vector<double> times_;
    std::vector<Sophus::SE3d> poses_;

    // 平移分量的插值器
    boost::math::interpolators::pchip interp_tx_;
    boost::math::interpolators::pchip interp_ty_;
    boost::math::interpolators::pchip interp_tz_;

    // 旋转部分：通过李代数表示进行插值
    boost::math::interpolators::pchip interp_rx_;
    boost::math::interpolators::pchip interp_ry_;
    boost::math::interpolators::pchip interp_rz_;

public:
    SE3Trajectory(const std::vector<double>& timestamps,
                  const std::vector<Sophus::SE3d>& poses)
        : times_(timestamps), poses_(poses)
    {
        std::vector<Eigen::Vector3d> rot_vecs;
        std::vector<Eigen::Vector3d> trans_vecs;

        // 将每个位姿的旋转部分转换为李代数（旋转向量）
        for (const auto& pose : poses_) {
            rot_vecs.push_back(pose.so3().log());
            trans_vecs.push_back(pose.translation());
        }

        // 提取各轴向的平移与旋转数据用于后续插值
        std::vector<double> tx, ty, tz, rx, ry, rz;
        for (size_t i = 0; i < timestamps.size(); ++i) {
            tx.push_back(trans_vecs[i].x());
            ty.push_back(trans_vecs[i].y());
            tz.push_back(trans_vecs[i].z());
            rx.push_back(rot_vecs[i].x());
            ry.push_back(rot_vecs[i].y());
            rz.push_back(rot_vecs[i].z());
        }

        // 构建逐维的PCHIP插值函数
        interp_tx_ = boost::math::interpolators::pchip(tx, timestamps);
        interp_ty_ = boost::math::interpolators::pchip(ty, timestamps);
        interp_tz_ = boost::math::interpolators::pchip(tz, timestamps);
        interp_rx_ = boost::math::interpolators::pchip(rx, timestamps);
        interp_ry_ = boost::math::interpolators::pchip(ry, timestamps);
        interp_rz_ = boost::math::interpolators::pchip(rz, timestamps);
    }

    // 根据时间t获取对应的SE(3)位姿
    Sophus::SE3d getPose(double t) const {
        // 插值得到当前时刻的平移向量
        Eigen::Vector3d trans(interp_tx_(t), interp_ty_(t), interp_tz_(t));
        
        // 插值得到当前时刻的旋转向量（在so(3)空间）
        Eigen::Vector3d rot_vec(interp_rx_(t), interp_ry_(t), interp_rz_(t));
        
        // 通过指数映射还原为SO(3)旋转矩阵
        Sophus::SO3d R = Sophus::SO3d::exp(rot_vec);

        // 组合成SE(3)变换
        return Sophus::SE3d(R, trans);
    }
};

说明：

• 将姿态中的旋转部分使用 SO(3) 的对数映射转换至李代数 so(3)，即旋转向量空间后进行插值处理。
• 平移分量则直接在欧几里得空间中按各坐标轴分别插值。
• 最终输出为 Sophus::SE3d 类型的对象，便于与点云、IMU等传感器数据进行融合计算。
• 该轨迹类适用于需要连续时间建模的系统，如LIO（Lidar-Inertial Odometry）或连续时间ICP（CT-ICP）框架中，可用于任意时刻的位姿预测。

3 使用示例

#include "SE3Trajectory.h"
#include "ContinuousTrajectory.h"

int main() {
    // 定义离散的时间戳
    std::vector<double> t = {0, 1, 2};

    
#include <boost/math/interpolators/...>

// 1. 三维空间位置轨迹
std::vector<Eigen::Vector3d> pos = { {0,0,0}, {1,1,0}, {2,0,1} };
ContinuousTrajectory traj(t, pos);
Eigen::Vector3d p = traj.getPosition(1.5);
Eigen::Vector3d v = traj.getVelocity(1.5);
std::cout << "p = " << p.transpose() << ", v = " << v.transpose() << std::endl;

// 2. SE3位姿轨迹构建
std::vector<Sophus::SE3d> poses;
poses.push_back(Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0,0,0)));
poses.push_back(Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1,1,0)));
poses.push_back(Sophus::SE3d(Eigen::Matrix3d::Identity(), Eigen::Vector3d(2,0,1)));

SE3Trajectory se3traj(t, poses);
Sophus::SE3d pose = se3traj.getPose(1.5);
std::cout << "pose translation = " << pose.translation().transpose() << std::endl;

4 扩展性建议

连续时间下的轨迹优化方法

可将当前的插值轨迹模型与IMU预积分产生的残差项进行融合，从而构建连续时间域上的优化框架。该方式能够提升状态估计精度，尤其适用于高动态运动或传感器异步场景。

旋转插值的精度增强策略

在处理姿态插值时，建议使用球面线性插值（SLERP）替代现有的PCHIP插值方法。SLERP能保证旋转路径的最短性和角速度的平滑性，在对运动连续性要求较高的应用中表现更优。

多阶运动状态的联合插值扩展

为进一步满足SLAM系统的需求，可在现有位置插值基础上，引入速度和加速度的插值支持。通过构造更高阶的样条曲线，实现对完整运动状态的精确重建，便于前端配准与后端优化模块直接调用。

线程安全与并行化查询能力

所采用的Boost插值库具备线程安全特性，允许多线程环境下并发执行轨迹查询操作。此特性可用于激光雷达多线束的时间补偿计算，显著提升数据处理效率，适合实时系统部署。

扫码加我 拉你入群

请注明：姓名-公司-职位

以便审核进群资格，未注明则拒绝

分享0 收藏0 回帖

关键词：interp inter ATION boost Math