B-Spline（B样条）插值代码实现

点云兔子

已于 2024-11-11 19:40:29 修改

阅读量2.2k

点赞数 13

分类专栏： SLAM基础文章标签： B-Spline 样条曲线插值

于 2024-11-10 17:00:54 首次发布

本文链接：https://blog.youkuaiyun.com/qq_40301351/article/details/143662284

版权

SLAM基础专栏收录该内容

13 篇文章

订阅专栏

B-Spline（B样条）详细介绍

B-Spline（B样条）是一种常用于计算机图形学和数据拟合的数学方法。它由一系列控制点和节点（Knots）以及一组基函数（Basis Functions）组成。B-Spline 能够通过控制点生成平滑曲线或曲面，广泛应用于图形建模、路径规划、数据插值等领域。

B-Spline 主要概念

控制点（Control Points）：控制点决定了样条曲线的形状。通过移动控制点，可以改变曲线的形状。
节点（Knots）：节点向量定义了样条曲线在参数空间中的分布，并且决定了每个控制点对样条曲线的影响范围。
基函数（Basis Functions）：B-Spline 使用一组基函数来构造样条曲线。通过加权控制点与相应的基函数，可以获得平滑的插值曲线。

B-Spline 的构造

B-Spline 曲线的数学公式通常表示为：

$\sum_{i=0}^{n} P_i B_{i,p}(t)$

其中：

$C (t)$ 是曲线， $P_i$ 是控制点。
$B_{i,p}(t)$ 是第 $i$ 个控制点的基函数，基函数的阶数为 $p$ ， $t$ 是参数。

阶数和度数

阶数（Order）：基函数的阶数，即基函数的次数加一。
度数（Degree）：度数通常定义为阶数减一，代表基函数的次数。

例如，三次B-Spline曲线的度数为3，阶数为4。

B-Spline 在 C++ 中的实现

1. BSpline.h 文件

该文件声明了 B-Spline 类，提供了初始化、计算和插值等功能。以下是 BSpline 类的定义：

#pragma once
#include <Eigen/Core>
#include <vector>

class BSpline {
public:
    // 构造函数：初始化 B-Spline 对象
    BSpline(int degree = 3);

    // 设置节点向量
    void setKnots(const Eigen::VectorXd &knots);

    // 设置控制点
    void setControlPoints(const Eigen::VectorXd &x, const Eigen::VectorXd &y);

    // 根据给定的参数生成节点向量
    void generateKnots(int n);

    // 生成样条基函数矩阵
    void generateSplineBasis();

    // 根据基函数进行插值
    void interpolate();

    // 获取插值后的 x 坐标
    std::vector<double> getSplineX() const;

    // 获取插值后的 y 坐标
    std::vector<double> getSplineY() const;

private:
    int degree;  // B-Spline 曲线的度数
    Eigen::VectorXd knots;  // 节点向量
    Eigen::VectorXd xControlPoints;  // x 控制点
    Eigen::VectorXd yControlPoints;  // y 控制点
    std::vector<std::vector<double>> splineBasis;  // 样条基函数矩阵
    std::vector<double> splineX;  // 插值后的 x 坐标
    std::vector<double> splineY;  // 插值后的 y 坐标
};

构造函数

BSpline(int degree = 3);

该构造函数用于初始化 B-Spline 对象，默认生成三次样条。degree 参数指定了样条的度数（默认为3，表示三次样条）。

成员函数

setKnots：设置节点向量。节点向量是样条曲线在参数空间中的分布，控制着每个基函数的范围。

void setKnots(const Eigen::VectorXd &knots);

参数：knots 是一个 Eigen::VectorXd 类型的向量，包含了样条的节点信息。
功能：通过该函数，可以手动设置节点向量。如果需要自定义节点分布，使用此函数。

setControlPoints：函数用于设置样条曲线的控制点。控制点是影响样条形状的关键，B-Spline 曲线通过控制点和样条基函数的加权求和来生成。。

void setControlPoints(const Eigen::VectorXd &x, const Eigen::VectorXd &y);

参数：
x：控制点的 x 坐标。
y：控制点的 y 坐标。
功能：该函数允许用户设置曲线的控制点，控制点越多，生成的曲线将越复杂，变化也越大。

generateKnots：函数根据给定的参数生成节点向量。这通常是在自动生成样条时使用。

void generateKnots(int n);

参数：n 表示生成的节点数。
功能：通过该函数，可以根据控制点数和样条度数自动生成节点向量。通常使用均匀分布的节点。

generateSplineBasis：函数生成样条基函数矩阵。样条基函数是用于计算每个控制点对曲线的贡献度的函数。

void generateSplineBasis();

功能：该函数根据控制点和节点向量生成样条基函数矩阵。在插值过程中，每个控制点与样条基函数的值相乘，然后加和，得到最终的曲线。

interpolate：函数通过样条基函数和控制点进行插值计算，生成最终的插值曲线。

void interpolate();

功能：该函数执行插值操作，计算所有控制点对应的插值结果，并将结果存储在内部数据结构中。

getSplineX 和 getSplineY：获取插值后的 x 和 y 坐标。

std::vector<double> getSplineX() const;
std::vector<double> getSplineY() const;

功能：这两个函数返回存储了插值结果的 x 和 y 坐标的向量，供用户进一步使用或绘图。

2. BSpline.cpp 文件

这是 BSpline 类的实现文件，包含了所有成员函数的具体实现。以下是文件的主要内容：

#include "BSpline.h"
#include <algorithm>
#include <stdexcept>
#include <iostream>

// 构造函数
BSpline::BSpline(int degree) : degree_(degree) {}

// 设置节点
void BSpline::setKnots(const Eigen::VectorXd& knots) {
    knots_ = knots;
}

// 设置控制点
void BSpline::setControlPoints(const Eigen::VectorXd& x, const Eigen::VectorXd& y) {
    x_ = x;
    y_ = y;
    if (x_.size() != y_.size()) {
        throw std::invalid_argument("x and y must have the same length.");
    }
}

// 生成节点向量
Eigen::VectorXd BSpline::generateKnots(int N, double a, double b, int method) const {
    int K = degree_ + 1;
    if (N < K) {
        throw std::invalid_argument("N must be greater than or equal to K.");
    }

    if (method == 1) {
        return Eigen::VectorXd::LinSpaced(N + K, a, b);  // 均匀样条
    } else {
        Eigen::VectorXd knots(N + K);
        knots.head(K - 1).setConstant(a);
        knots.tail(K - 1).setConstant(b);
        knots.segment(K - 1, N - K + 2) = Eigen::VectorXd::LinSpaced(N - K + 2, a, b);
        return knots;  // 准均匀样条
    }
}

Eigen::MatrixXd BSpline::generateSplineBasis(const Eigen::VectorXd& pts) const {
    int N = pts.size();  // 输入点数量
    int M = knots_.size();  // 节点数量

    // 打印调试信息
    // std::cout << "Number of points (N): " << N << std::endl;
    // std::cout << "Number of knots (M): " << M << std::endl;

    // 确保节点的数量至少大于1，否则无法生成样条基
    if (M <= 1) {
        std::cerr << "Error: Number of knots must be greater than 1." << std::endl;
        return Eigen::MatrixXd();
    }

    // 初始化样条基函数矩阵
    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(N, M - 1);  

    // 打印 B 矩阵的大小
    // std::cout << "Matrix B size: " << B.rows() << " x " << B.cols() << std::endl;

    // 初始化 1 阶样条基
    for (int i = 0; i < M - 1; ++i) {
        for (int j = 0; j < N; ++j) {
            if (pts[j] >= knots_[i] && pts[j] < knots_[i + 1]) {
                B(j, i) = 1.0;
            }
        }
    }

    // 确保最后一个基函数正确
    if (N > 0 && M > 1) {
        B(N - 1, M - 2) = 1.0;
    }

    // 打印基函数矩阵更新后的状态
    // std::cout << "Updated B matrix after initialization:" << std::endl;
    // std::cout << B << std::endl;

    // 递归计算 k 阶样条基
    int K = degree_ + 1;  // 样条的阶数
    for (int k_ = 2; k_ <= K; ++k_) {
        for (int i = 0; i < M - k_; ++i) {
            for (int j = 0; j < N; ++j) {
                double c1 = (knots_[i + k_ - 1] != knots_[i]) ? (pts[j] - knots_[i]) / (knots_[i + k_ - 1] - knots_[i]) : 0.0;
                double c2 = (knots_[i + k_] != knots_[i + 1]) ? (knots_[i + k_] - pts[j]) / (knots_[i + k_] - knots_[i + 1]) : 0.0;
                // 更新样条基函数矩阵
                B(j, i) = c1 * B(j, i) + c2 * B(j, i + 1);
            }
        }
    }

    // 打印递归计算后的 B 矩阵
    // std::cout << "Updated B matrix after recursion:" << std::endl;
    // std::cout << B << std::endl;

    // 返回前 M-K 列的矩阵（去掉多余的列）
    return B.leftCols(M - K);
}


// 获取样条曲线的 x 坐标
Eigen::VectorXd BSpline::getSplineX(const Eigen::VectorXd& pts) {
    Eigen::MatrixXd B = generateSplineBasis(pts);
    return B * x_;
}

// 获取样条曲线的 y 坐标
Eigen::VectorXd BSpline::getSplineY(const Eigen::VectorXd& pts) {
    Eigen::MatrixXd B = generateSplineBasis(pts);
    return B * y_;
}

// 插值函数
Eigen::VectorXd BSpline::interpolate(const Eigen::VectorXd& u) {
    Eigen::VectorXd xx = getSplineX(u);
    Eigen::VectorXd yy = getSplineY(u);
    Eigen::VectorXd v(u.size());

    for (int i = 0; i < u.size(); ++i) {
        // 找到 u[i] 在 xx 中的位置，保证 idx 不会越界
        int idx = (std::upper_bound(xx.data(), xx.data() + xx.size(), u[i]) - xx.data()) - 1;

        // 确保 idx 在合法范围内
        if (idx < 0) {
            idx = 0;  // 如果 u[i] 小于 xx[0]，则使用最左边的值
        }
        if (idx >= xx.size() - 1) {
            idx = xx.size() - 2;  // 如果 u[i] 大于 xx[xx.size() - 1]，则使用最右边的值
        }

        // 计算 t 并检查是否有除零错误
        double denominator = xx[idx + 1] - xx[idx];
        double t = 0.0;  // 必须在这里声明并初始化 t
        if (denominator != 0) {
            t = (u[i] - xx[idx]) / denominator;  // 如果分母不为零，计算 t
        }

        // 计算插值
        v[i] = (1 - t) * yy[idx] + t * yy[idx + 1];
    }

    return v;
}

3. 应用实例

假设有以下控制点和节点，并希望通过 B-Spline 进行插值：

#include <iostream>
#include "BSpline.h"
#include "matplotlibcpp.h"
#include <vector>  // 引入 std::vector
#include <Eigen/Dense>

namespace plt = matplotlibcpp;

int main1() {
    // 初始化样条
    BSpline bspline(3);
    Eigen::VectorXd x(8), y(8);
    x << 58.5, 61.5, 64.5, 67.5, 70.5, 73.0, 75.0, 77.0;
    y << 225.0917, 289.8264, 369.8764, 397.5458, 423.6194, 398.9139, 369.1653, 309.0236;

    // 设置控制点
    bspline.setControlPoints(x, y);

    // 生成节点（knots），并打印调试信息
    Eigen::VectorXd knots = bspline.generateKnots(8, 0.0, 1.0, 2);
    std::cout << "Generated Knots: " << knots.transpose() << std::endl;

    // 设置节点
    bspline.setKnots(knots);

    // 生成样条曲线（插值点），并打印调试信息
    Eigen::VectorXd pts = Eigen::VectorXd::LinSpaced(100, 0.0, 1.0);
    Eigen::VectorXd xx = bspline.getSplineX(pts);
    Eigen::VectorXd yy = bspline.getSplineY(pts);
    
    std::cout << "Spline X values: " << xx.transpose() << std::endl;
    std::cout << "Spline Y values: " << yy.transpose() << std::endl;

    //插值时确保u的范围在xx的最小最大值之间
    Eigen::VectorXd u = Eigen::VectorXd::LinSpaced(50, std::max(xx.minCoeff(), 0.0), std::min(xx.maxCoeff(), 1.0));
    Eigen::VectorXd v = bspline.interpolate(u);

    // 将 Eigen::VectorXd 转换为 std::vector<double>
    std::vector<double> xx_vec(xx.data(), xx.data() + xx.size());
    std::vector<double> yy_vec(yy.data(), yy.data() + yy.size());
    std::vector<double> u_vec(u.data(), u.data() + u.size());
    std::vector<double> v_vec(v.data(), v.data() + v.size());

    // 绘制样条曲线和插值点
    plt::figure();
    plt::plot(xx_vec, yy_vec, "-");
    plt::scatter(u_vec, v_vec, 10);  // 使用 std::vector<double>
    plt::show();

    return 0;
}


int main() {
    // 控制点 (x 和 y)
    Eigen::VectorXd x(8);
    Eigen::VectorXd y(8);
    x << 0, 0.05, 0.20, 0.3, 0.40, 0.60, 0.70, 0.95;
    y << 0, 0.20, 0.35, 0.4, 0.42, 0.43, 0.44, 0.45;

    // 打印控制点的维度
    std::cout << "Control points x size: " << x.size() << std::endl;
    std::cout << "Control points y size: " << y.size() << std::endl;

    // 创建样条对象
    BSpline spline(3);  // 默认三次样条
    spline.setControlPoints(x, y);

    // 生成节点
    Eigen::VectorXd knots = spline.generateKnots(x.size(), x(0), x(x.size() - 1));

    // 打印生成的节点
    std::cout << "Generated knots size: " << knots.size() << std::endl;
    spline.setKnots(knots);

    // 生成样条基
    Eigen::VectorXd pts = Eigen::VectorXd::LinSpaced(100, x(0), x(x.size() - 1));
    Eigen::MatrixXd B = spline.generateSplineBasis(pts);

    // 打印样条基矩阵的维度
    std::cout << "Spline basis B size: " << B.rows() << "x" << B.cols() << std::endl;

    // 将 Eigen::VectorXd 转换为 std::vector<double>
    std::vector<double> x_vec(x.data(), x.data() + x.size());
    std::vector<double> y_vec(y.data(), y.data() + y.size());

    // 使用 matplotlibcpp 绘制散点图
    std::map<std::string, std::string> params;
    params["color"] = "r";  // 红色
    plt::scatter(x_vec, y_vec, 10, params);  // 使用参数映射来设置颜色

    // 绘制样条曲线
    Eigen::VectorXd spline_x = spline.getSplineX(pts);
    Eigen::VectorXd spline_y = spline.getSplineY(pts);

    // 打印样条曲线的 X 和 Y 值
    // std::cout << "Spline X values: " << spline_x.transpose() << std::endl;
    // std::cout << "Spline Y values: " << spline_y.transpose() << std::endl;
 
    // 转换为 std::vector
    std::vector<double> spline_x_vec(spline_x.data(), spline_x.data() + spline_x.size());
    std::vector<double> spline_y_vec(spline_y.data(), spline_y.data() + spline_y.size());

    // 绘制样条曲线
    plt::plot(spline_x_vec, spline_y_vec, "b-");  // "b-" 表示蓝色实线

    // 显示图像
    plt::show();

    return 0;
}