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

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. 最常用插值器一览

以下 4 类是工程中最常用的：

2.1 分段线性：`boost::math::interpolators::piecewise_linear`

适合：

简单插值
控制流平滑
SLAM 中对激光点做时间插值（扫描补偿）

特点：

O(1) lookup
无需计算导数
单调性自动保证

2.2 单调三次样条 PCHIP：`pchip`

适合：

物理数据
激光里程计（LIO）中插值旋转或位置
避免 overshoot 不可信的振荡

特点：

单调区间不会产生振荡
比普通 cubic spline 更稳定

2.3 Akima 样条：`akima`

适合：

有噪声的测量序列
GPS、IMU、Odom 时间同步
轨迹平滑

优点：

对异常点不敏感
局部控制性强

2.4 有理插值（Barycentric rational）：`barycentric_rational`

适合：

高精度科学计算
已知点数不多但要求高精度

3. 每种插值的使用详解

3.1 分段线性插值（piecewise_linear）

cpp 复制代码

#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 front-end 激光点时间补偿

3.2 单调三次样条 PCHIP 插值

cpp 复制代码

#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 速度插值
激光线束去畸变（确保 monotonic timestamp）

3.3 Akima 插值

cpp 复制代码

#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 连续）

适合 SLAM：

IMU + LiDAR 外推
Robot arm 轨迹 smoothing
建图轨迹 post-processing

3.4 Barycentric Rational 有理插值

cpp 复制代码

#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};

    auto interp = boost::math::interpolators::barycentric_rational(xs, ys);

    std::cout << interp(1.25) << std::endl;
}

优点

数值精度高
适用于 smooth 函数
常用于科学计算 / 数值代数

4. 插值器的导数计算

多数插值器支持导数，例如：

cpp 复制代码

interp.prime(x)       // 一阶导
interp.double_prime(x)  // 二阶导

用于：

优化
ICP 线性化
时间连续轨迹（continuous-time SLAM）

5. 插值器对 SLAM 的常见用途

用途	推荐插值器
激光扫描去畸变（scan deskew）	piecewise_linear or pchip
IMU 姿态插值（SO(3)）	pchip (对角度)
IMU 位置/速度连续逼近	akima
回环检测前对轨迹 smoothing	akima
多传感器时间同步	piecewise_linear
后端优化的初始轨迹生成	pchip / akima

6. Boost 插值的性能与复杂度

插值方式	构建复杂度	查询复杂度	总体性能
分段线性	O(N)	O(1)	最高
PCHIP	O(N)	O(1)	稳定、高质量
Akima	O(N)	O(1)	比 cubic spline 更稳
Barycentric	O(N^2)	O(N)	高精度计算用途

7. 应用示例

1 连续时间轨迹类模板（位置/速度插值）

cpp 复制代码

#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, timestamps);
        interp_y_ = boost::math::interpolators::pchip(y, timestamps);
        interp_z_ = boost::math::interpolators::pchip(z, timestamps);
    }

    // 获取位置
    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 保证位置插值单调、平滑。
可直接获取速度（导数），方便连续时间 SLAM 或 IMU 前向预测。
可拓展加入加速度插值。

SO(3)/SE(3) 插值模板（Boost + Sophus）

cpp 复制代码

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

class SE3Trajectory {
public:
    SE3Trajectory(const std::vector<double>& timestamps,
                  const std::vector<Sophus::SE3d>& poses)
        : times_(timestamps), poses_(poses)
    {
        // 对旋转做 log 映射到 Lie algebra
        std::vector<Eigen::Vector3d> rot_vecs;
        std::vector<Eigen::Vector3d> trans_vecs;
        for (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());
        }

        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);
    }

    Sophus::SE3d getPose(double t) const {
        Eigen::Vector3d trans(interp_tx_(t), interp_ty_(t), interp_tz_(t));
        Eigen::Vector3d rot_vec(interp_rx_(t), interp_ry_(t), interp_rz_(t));
        Sophus::SO3d R = Sophus::SO3d::exp(rot_vec);
        return Sophus::SE3d(R, trans);
    }

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_;

    // 旋转（Lie algebra 旋转向量）
    boost::math::interpolators::pchip interp_rx_;
    boost::math::interpolators::pchip interp_ry_;
    boost::math::interpolators::pchip interp_rz_;
};

说明：

将旋转用 SO(3) log 映射到 Lie algebra 后插值。
平移直接插值。
输出为 SE3d 对象，便于直接与点云/IMU 数据融合。
可在 LIO/CT-ICP 系统中直接调用进行连续时间位姿预测。

3 使用示例

cpp 复制代码

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

int main() {
    // 1. 位置轨迹
    std::vector<double> t = {0, 1, 2};
    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，对速度连续性要求高时更适合。
多维插值扩展 ：

可加入速度、加速度插值，便于自己SLAM系统直接使用。
线程安全 / 并行查询 ：

Boost 插值器查询线程安全，可并行计算多激光线束补偿。

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

1. Boost.Math 插值模块概览

2. 最常用插值器一览

2.1 分段线性：boost::math::interpolators::piecewise_linear

2.2 单调三次样条 PCHIP：pchip

2.3 Akima 样条：akima

2.4 有理插值（Barycentric rational）：barycentric_rational

3. 每种插值的使用详解

3.1 分段线性插值（piecewise_linear）

3.2 单调三次样条 PCHIP 插值

3.3 Akima 插值

3.4 Barycentric Rational 有理插值

4. 插值器的导数计算

5. 插值器对 SLAM 的常见用途

6. Boost 插值的性能与复杂度

7. 应用示例

1 连续时间轨迹类模板（位置/速度插值）

SO(3)/SE(3) 插值模板（Boost + Sophus）

3 使用示例

4 拓展建议

2.1 分段线性：`boost::math::interpolators::piecewise_linear`

2.2 单调三次样条 PCHIP：`pchip`

2.3 Akima 样条：`akima`

2.4 有理插值（Barycentric rational）：`barycentric_rational`