最近工作要用B样条曲线，就花时间研究了下。
生成B样条曲线 首先需要有一系列控制点，然后在B样条曲线看来，绘制主要就是插值。整体思想是按照一定的顺序把控制点投影到一个一维区间，控制点投影到一维区域所在的位置叫做节点，和一一对应。在这个一维区间上均匀取值，然后计算出在原空间对应的位置即可得到 。
控制点和节点对应关系
现在有一系列高维空间控制点，一种很显然的方式是将他们按照顺序对应为一维空间的，考虑到阶B样条插值每个点需要个节点来控制生成，因此为了生成头尾两个控制点，节点的数量为，一般节点的生成方法有均匀法，这里为了过头尾两个控制点，将节点设置成，前后各有个和，分别用来生成第一个控制点和最后一个控制点。
然后从区间里面均匀采样，比如采样到的位置，该位置对应的高维空间点为
可以看到其中的关键是求解控制点对应的贡献或者说是权重 。这里首先来看0阶的权重，如下所示
这可以认为是一个最近邻插值，实现后的效果类似信号处理里面0阶保持。
高阶的权重通过如下的递推式子得到
如果把 简写成 ，则有
按照上面的公式得到的C++程序如下所示
void BSpline(vector &point_x, vector &point_y,
vector &plan_path_x, vector &plan_path_y, int order = 3) {
int knot_parameter = point_x.size() + order;
vector knot(knot_parameter + 1, 0.0);
vector b(knot_parameter * (order + 1), 0.0);
for (int i = order; i < knot.size(); ++i)
knot[i] = (min((double)point_x.size(), i + 0.0) - order) / ((double)point_x.size() - order);
for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
double t = i / (plan_path_x.size() - 0.0);
for (int j = 0; j < knot_parameter; ++j) {
if (knot[j] <= t && knot[j + 1] > t) b[j] = 1.0;
else b[j] = 0.0;
}
for (int deg = 1; deg <= order; ++deg) {
for (int j = 0; j + deg < knot_parameter; ++j) {
b[deg * knot_parameter + j] = 0.0;
if (knot[j + deg] != knot[j])
b[deg * knot_parameter + j] +=
(t - knot[j]) / (knot[j + deg] - knot[j]) * b[(deg - 1) * knot_parameter + j];
if (knot[j + deg + 1] != knot[j + 1])
b[deg * knot_parameter + j] +=
(knot[j + deg + 1] - t) / (knot[j + deg + 1] - knot[j + 1]) * b[(deg - 1) * knot_parameter + j + 1];
}
}
plan_path_x[i] = 0.0;
plan_path_y[i] = 0.0;
for (int j = 0; j < point_x.size(); ++j) {
plan_path_x[i] += point_x[j] * b[order * knot_parameter + j];
plan_path_y[i] += point_y[j] * b[order * knot_parameter + j];
}
}
plan_path_x.back() = point_x.back();
plan_path_y.back() = point_y.back();
}
注意到系数矩阵b当前的值只取决于的值和前面更前面的值无关，按照动态规划的常规套路可以将系数矩阵进行压缩，优化后的代码如下所示
void BSpline(vector &point_x, vector &point_y,
vector &plan_path_x, vector &plan_path_y, int order = 3) {
int knot_parameter = point_x.size() + order;
vector knot(knot_parameter + 1, 0.0);
vector b(knot_parameter, 0.0);
for (int i = order; i < knot.size(); ++i)
knot[i] = (min((double)point_x.size(), i + 0.0) - order) / ((double)point_x.size() - order);
for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
double t = i / (plan_path_x.size() - 0.0);
for (int j = 0; j < knot_parameter; ++j) {
if (knot[j] <= t && knot[j + 1] > t) b[j] = 1.0;
else b[j] = 0.0;
}
for (int deg = 1; deg <= order; ++deg) {
for (int j = 0; j + deg < knot_parameter; ++j) {
if (knot[j + deg] != knot[j])
b[j] = (t - knot[j]) / (knot[j + deg] - knot[j]) * b[j];
else b[j] = 0.0;
if (knot[j + deg + 1] != knot[j + 1])
b[j] += (knot[j + deg + 1] - t) / (knot[j + deg + 1] - knot[j + 1]) * b[j + 1];
}
}
plan_path_x[i] = 0.0;
plan_path_y[i] = 0.0;
for (int j = 0; j < point_x.size(); ++j) {
plan_path_x[i] += point_x[j] * b[j];
plan_path_y[i] += point_y[j] * b[j];
}
}
plan_path_x.back() = point_x.back();
plan_path_y.back() = point_y.back();
}
注意到根据前面的0阶表达式，每个 只有一个 不为0，剩下的均为0，因此在阶数较小的情况下可以考虑直接写出表达式，注意到下面表达式中的均满足。
考虑到，因此0阶情况下的表达式为
考虑到，因此1阶情况下的表达式为
考虑到，因此2阶情况下的表达式为
考虑到，因此3阶情况下的表达式为
注意到每个点只由个控制点生成，计算系数的时候不需要遍历整个节点和控制点，只需要遍历你所需要的那几个即可，因此前面的代码可以优化为
void BSpline(vector &point_x, vector &point_y,
vector &plan_path_x, vector &plan_path_y, int order = 3) {
int cur_index = 0;
int knot_parameter = point_x.size() + order;
vector knot(knot_parameter + 1, 0.0);
vector b(knot_parameter, 0.0);
for (int i = order; i < knot.size(); ++i)
knot[i] = (min((double)point_x.size(), i + 0.0) - order) / ((double)point_x.size() - order);
for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
double t = i / (plan_path_x.size() - 0.0);
while (knot[cur_index] <= t) ++cur_index;
b[cur_index - 1] = 1.0;
for (int deg = 1; deg <= order; ++deg) {
for (int j = cur_index - deg - 1; j < cur_index; ++j) {
if (knot[j + deg] != knot[j])
b[j] = (t - knot[j]) / (knot[j + deg] - knot[j]) * b[j];
else b[j] = 0.0;
if (knot[j + deg + 1] != knot[j + 1])
b[j] += (knot[j + deg + 1] - t) / (knot[j + deg + 1] - knot[j + 1]) * b[j + 1];
}
}
plan_path_x[i] = 0.0;
plan_path_y[i] = 0.0;
for (int j = cur_index - order - 1; j < cur_index; ++j) {
plan_path_x[i] += point_x[j] * b[j];
plan_path_y[i] += point_y[j] * b[j];
b[j] = 0.0;
}
}
plan_path_x.back() = point_x.back();
plan_path_y.back() = point_y.back();
}
前面的公式
从数学上来看，系数是由上一层和两者共同组成，从另一个角度来看的系数支持了下一层的和两者。前面的两段代码采用的是前者的写法，考虑到整个数据最开始只有一个地方为1，因此采用后者写法可以进一步减少计算量，只计算已知非0值的位置。后一种写法的数学公式表达如下
注意到上式中的系数满足，在计算时可以利用该性质，减少计算量。
进一步优化过后的代码如下
void BSpline(vector &point_x, vector &point_y,
vector &plan_path_x, vector &plan_path_y, int order = 3) {
int cur_index = 0;
int knot_parameter = point_x.size() + order;
vector knot(knot_parameter + 1, 0.0);
vector b(knot_parameter, 0.0);
for (int i = order; i < knot.size(); ++i)
knot[i] = (min((double)point_x.size(), i + 0.0) - order) / ((double)point_x.size() - order);
for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
double t = i / (plan_path_x.size() - 0.0);
while (knot[cur_index] <= t) ++cur_index;
b[cur_index - 1] = 1.0;
for (int deg = 0; deg < order; ++deg) {
for (int j = cur_index - deg - 1; j < cur_index; ++j) {
double p = 0.0;
if (knot[j + deg + 1] != knot[j])
p = (knot[j + deg + 1] - t) / (knot[j + deg + 1] - knot[j]);
b[j - 1] += p * b[j];
b[j] *= (1.0 - p);
}
}
plan_path_x[i] = 0.0;
plan_path_y[i] = 0.0;
for (int j = cur_index - order - 1; j < cur_index; ++j) {
plan_path_x[i] += point_x[j] * b[j];
plan_path_y[i] += point_y[j] * b[j];
b[j] = 0.0;
}
}
plan_path_x.back() = point_x.back();
plan_path_y.back() = point_y.back();
}
继续优化得到代码
void BSpline(vector& point_x, vector& point_y,
vector& plan_path_x, vector& plan_path_y, int order = 3) {
int cur_index = 0;
double step = (point_x.size() - order + 0.0) / plan_path_x.size();
int knot_parameter = point_x.size() + order;
vector knot(knot_parameter + 1, 0.0);
vector b(knot_parameter, 0.0);
for (int i = order; i < knot.size(); ++i) knot[i] = min(i, (int)point_x.size()) - order;
for (int i = 0; i + 1 < plan_path_x.size(); ++i) {
double t = i * step;
while (knot[cur_index] <= t) ++cur_index;
b[cur_index - 1] = 1.0;
for (int deg = 0; deg < order; ++deg) {
for (int j = cur_index - deg - 1; j < cur_index; ++j) {
int knot_temp = knot[j + deg + 1];
double p = knot_temp - knot[j];
if (p) p = (knot_temp - t) / p;
b[j - 1] += p * b[j];
b[j] *= (1.0 - p);
}
}
plan_path_x[i] = 0.0;
plan_path_y[i] = 0.0;
for (int j = cur_index - order - 1; j < cur_index; ++j) {
plan_path_x[i] += point_x[j] * b[j];
plan_path_y[i] += point_y[j] * b[j];
b[j] = 0.0;
}
}
plan_path_x.back() = point_x.back();
plan_path_y.back() = point_y.back();
}
注意到设置的时候，除了头尾添加的0和1以外，节点等分了区间，利用该性质对前面的递推公式做变换可以得到
令$$
没时间写了，先放上程序再说
void BSpline(vector& point_x, vector& point_y,
vector& plan_path_x, vector& plan_path_y, int order = 3) {
int cur_index = 0;
double step = (point_x.size() - 1.0) / (plan_path_x.size() - 1.0);
vector knot(point_x.size() + 2 * order, 0.0);
vector b(point_x.size() + order, 0.0);
for (int i = 0; i < knot.size(); ++i) knot[i] = i + 1 - order;
for (int i = 1; i + 1 < plan_path_x.size(); ++i) {
double t = i * step;
while (knot[cur_index] <= t) ++cur_index;
b[cur_index] = 1.0;
for (int deg = 0; deg < order; ++deg) {
for (int j = cur_index - deg; j <= cur_index; ++j) {
int knot_temp = knot[j + deg];
double p = (knot_temp - t) / (knot_temp - knot[j - 1]);
b[j - 1] += p * b[j];
b[j] *= (1.0 - p);
}
}
plan_path_x[i] = 0.0;
plan_path_y[i] = 0.0;
for (int j = cur_index - order; j <= cur_index; ++j) {
int point_index = min(max(j - order / 2, 0), (int)point_x.size() - 1);
plan_path_x[i] += point_x[point_index] * b[j];
plan_path_y[i] += point_y[point_index] * b[j];
b[j] = 0.0;
}
}
plan_path_x[0] = point_x[0];
plan_path_y[0] = point_y[0];
plan_path_x.back() = point_x.back();
plan_path_y.back() = point_y.back();
}
void BSpline3(vector& point_x, vector& point_y,
vector& plan_path_x, vector& plan_path_y, int order = 3) {
int cur_index = 0;
double step = (point_x.size() - 1.0) / (plan_path_x.size() - 1.0);
vector point_x_extended(point_x.size() + order, 0.0);
vector point_y_extended(point_y.size() + order, 0.0);
vector knot(point_x.size() + order, 0.0);
vector b(order + 1, 0.0);
for (int i = 0; i < knot.size(); ++i) knot[i] = i + 1 - order;
for (int i = 0; i < point_x_extended.size(); ++i) {
int ii = i - order / 2;
if (ii < 0) {
point_x_extended[i] = 2 * point_x[0] - point_x[1];
point_y_extended[i] = 2 * point_y[0] - point_y[1];
continue;
}
if (ii >= point_x.size()) {
point_x_extended[i] = 2 * point_x[point_x.size() - 1] - point_x[point_x.size() - 2];
point_y_extended[i] = 2 * point_y[point_x.size() - 1] - point_y[point_x.size() - 2];
continue;
}
point_x_extended[i] = point_x[ii];
point_y_extended[i] = point_y[ii];
}
for (int i = 0; i < plan_path_x.size(); ++i) {
double t = i * step;
while (knot[cur_index] <= t) ++cur_index;
t = t - knot[cur_index - 1];
b[0] = (-t * t * t + 3 * t * t - 3 * t + 1) / 6.0;
b[1] = (3 * t * t * t - 6 * t * t + 4) / 6.0;
b[2] = (-3 * t * t * t + 3 * t * t + 3 * t + 1) / 6.0;
b[3] = t * t * t / 6.0;
plan_path_x[i] = 0.0;
plan_path_y[i] = 0.0;
for (int j = 0; j <= order; ++j) {
plan_path_x[i] += point_x_extended[cur_index - order + j] * b[j];
plan_path_y[i] += point_y_extended[cur_index - order + j] * b[j];
}
}
}