1 /****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * This class implements the B-Spline
33 *
34 * Authors:
35 * Nicolas Melchior
36 *
37 *****************************************************************************/
38
39 #include <visp3/core/vpBSpline.h>
40 #include <visp3/core/vpDebug.h>
41
42 /*!
43 Basic constructor.
44
45 The degree \f$ p \f$ of the B-Spline basis functions is set to 3 to
46 compute cubic B-Spline.
47 */
vpBSpline()48 vpBSpline::vpBSpline()
49 : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
50 crossingPoints()
51 {
52 }
53
54 /*!
55 Copy constructor.
56
57 */
vpBSpline(const vpBSpline & bspline)58 vpBSpline::vpBSpline(const vpBSpline &bspline)
59 : controlPoints(bspline.controlPoints), knots(bspline.knots), p(bspline.p), // By default : p=3 for clubic spline
60 crossingPoints(bspline.crossingPoints)
61 {
62 }
63 /*!
64 Basic destructor.
65 */
~vpBSpline()66 vpBSpline::~vpBSpline() {}
67
68 /*!
69 Find the knot interval in which the parameter \f$ l_u \f$ lies. Indeed \f$
70 l_u \in [u_i, u_{i+1}[ \f$
71
72 Example : The knot vector is the following \f$ U = \{0, 0 , 1 , 2 ,3 , 3\}
73 \f$ with \f$ p \f$ is equal to 1.
74 - For \f$ l_u \f$ equal to 0.5 the method will retun 1.
75 - For \f$ l_u \f$ equal to 2.5 the method will retun 3.
76 - For \f$ l_u \f$ equal to 3 the method will retun 3.
77
78 \param l_u : The knot whose knot interval is seeked.
79 \param l_p : Degree of the B-Spline basis functions.
80 \param l_knots : The knot vector
81
82 \return the number of the knot interval in which \f$ l_u \f$ lies.
83 */
findSpan(double l_u,unsigned int l_p,std::vector<double> & l_knots)84 unsigned int vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots)
85 {
86 unsigned int m = (unsigned int)l_knots.size() - 1;
87
88 if (l_u > l_knots.back()) {
89 // vpTRACE("l_u higher than the maximum value in the knot vector :
90 // %lf",l_u);
91 return ((unsigned int)(m - l_p - 1));
92 }
93
94 // if (l_u == l_knots.back())
95 if (std::fabs(l_u - l_knots.back()) <=
96 std::fabs(vpMath::maximum(l_u, l_knots.back())) * std::numeric_limits<double>::epsilon())
97 return ((unsigned int)(m - l_p - 1));
98
99 double low = l_p;
100 double high = m - l_p;
101 double middle = (low + high) / 2.0;
102
103 while (l_u < l_knots[(unsigned int)middle] ||
104 l_u >= l_knots[(unsigned int)middle + 1]) {
105 if (l_u < l_knots[(unsigned int)vpMath::round(middle)])
106 high = middle;
107 else
108 low = middle;
109 middle = (low + high) / 2.0;
110 }
111
112 return (unsigned int)middle;
113 }
114
115 /*!
116 Find the knot interval in which the parameter \f$ u \f$ lies. Indeed \f$ u
117 \in [u_i, u_{i+1}[ \f$
118
119 Example : The knot vector is the following \f$ U = \{0, 0 , 1 , 2 ,3 , 3\}
120 \f$ with \f$ p \f$ is equal to 1.
121 - For \f$ u \f$ equal to 0.5 the method will retun 1.
122 - For \f$ u \f$ equal to 2.5 the method will retun 3.
123 - For \f$ u \f$ equal to 3 the method will retun 3.
124
125 \param u : The knot whose knot interval is seeked.
126
127 \return the number of the knot interval in which \f$ u \f$ lies.
128 */
findSpan(double u)129 unsigned int vpBSpline::findSpan(double u) { return findSpan(u, p, knots); }
130
131 /*!
132 Compute the nonvanishing basis functions at \f$ l_u \f$ which is in the \f$
133 l_i \f$ th knot interval. All the basis functions are stored in an array
134 such as :
135
136 N = \f$ N_{l_i,0}(l_u) \f$, \f$ N_{l_i-1,1}(l_u) \f$, \f$ N_{l_i,1}(l_u)
137 \f$, ... , \f$ N_{l_i-k,k}(l_u) \f$, ..., \f$ N_{l_i,k}(l_u) \f$, ... , \f$
138 N_{l_i-p,p}(l_u) \f$, ... , \f$ N_{l_i,p}(l_u) \f$
139
140 \param l_u : A real number which is between the extrimities of the knot
141 vector \param l_i : the number of the knot interval in which \f$ l_u \f$
142 lies \param l_p : Degree of the B-Spline basis functions. \param l_knots :
143 The knot vector
144
145 \return An array containing the nonvanishing basis functions at \f$ l_u \f$.
146 The size of the array is \f$ l_p +1 \f$.
147 */
computeBasisFuns(double l_u,unsigned int l_i,unsigned int l_p,std::vector<double> & l_knots)148 vpBasisFunction *vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p,
149 std::vector<double> &l_knots)
150 {
151 vpBasisFunction *N = new vpBasisFunction[l_p + 1];
152
153 N[0].value = 1.0;
154
155 double *left = new double[l_p + 1];
156 double *right = new double[l_p + 1];
157 double temp = 0.0;
158
159 for (unsigned int j = 1; j <= l_p; j++) {
160 left[j] = l_u - l_knots[l_i + 1 - j];
161 right[j] = l_knots[l_i + j] - l_u;
162 double saved = 0.0;
163
164 for (unsigned int r = 0; r < j; r++) {
165 temp = N[r].value / (right[r + 1] + left[j - r]);
166 N[r].value = saved + right[r + 1] * temp;
167 saved = left[j - r] * temp;
168 }
169 N[j].value = saved;
170 }
171 for (unsigned int j = 0; j < l_p + 1; j++) {
172 N[j].i = l_i - l_p + j;
173 N[j].p = l_p;
174 N[j].u = l_u;
175 N[j].k = 0;
176 }
177
178 delete[] left;
179 delete[] right;
180
181 return N;
182 }
183
184 /*!
185 Compute the nonvanishing basis functions at \f$ u \f$. All the basis
186 functions are stored in an array such as :
187
188 N = \f$ N_{i,0}(u) \f$, \f$ N_{i-1,1}(u) \f$, \f$ N_{i,1}(u) \f$, ... , \f$
189 N_{i-k,k}(u) \f$, ..., \f$ N_{i,k}(u) \f$, ... , \f$ N_{i-p,p}(u) \f$, ... ,
190 \f$ N_{i,p}(u) \f$
191
192 where i the number of the knot interval in which \f$ u \f$ lies.
193
194 \param u : A real number which is between the extrimities of the knot vector
195
196 \return An array containing the nonvanishing basis functions at \f$ u \f$.
197 The size of the array is \f$ p +1 \f$.
198 */
computeBasisFuns(double u)199 vpBasisFunction *vpBSpline::computeBasisFuns(double u)
200 {
201 unsigned int i = findSpan(u);
202 return computeBasisFuns(u, i, p, knots);
203 }
204
205 /*!
206 Compute the nonzero basis functions and their derivatives until the \f$
207 l_der \f$ th derivative. All the functions are computed at l_u.
208
209 \warning \f$ l_der \f$ must be under or equal \f$ l_p \f$.
210
211 The result is given as an array of size l_der+1 x l_p+1. The kth line
212 corresponds to the kth basis functions derivatives.
213
214 The formula to compute the kth derivative at \f$ u \f$ is :
215
216 \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} -
217 \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
218
219 where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$
220 p \f$ is the degree of the B-Spline basis function.
221
222 \param l_u : A real number which is between the extrimities of the knot
223 vector \param l_i : the number of the knot interval in which \f$ l_u \f$
224 lies \param l_p : Degree of the B-Spline basis functions. \param l_der : The
225 last derivative to be computed. \param l_knots : The knot vector
226
227 \return the basis functions and their derivatives as an array of size
228 l_der+1 x l_p+1. The kth line corresponds to the kth basis functions
229 derivatives.
230
231 Example : return[0] is the list of the 0th derivatives ie the basis
232 functions. return[k] is the list of the kth derivatives.
233 */
computeDersBasisFuns(double l_u,unsigned int l_i,unsigned int l_p,unsigned int l_der,std::vector<double> & l_knots)234 vpBasisFunction **vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
235 std::vector<double> &l_knots)
236 {
237 vpBasisFunction **N;
238 N = new vpBasisFunction *[l_der + 1];
239 for (unsigned int j = 0; j <= l_der; j++)
240 N[j] = new vpBasisFunction[l_p + 1];
241
242 vpMatrix a(2, l_p + 1);
243 vpMatrix ndu(l_p + 1, l_p + 1);
244 ndu[0][0] = 1.0;
245
246 double *left = new double[l_p + 1];
247 double *right = new double[l_p + 1];
248 double temp = 0.0;
249
250 for (unsigned int j = 1; j <= l_p; j++) {
251 left[j] = l_u - l_knots[l_i + 1 - j];
252 right[j] = l_knots[l_i + j] - l_u;
253 double saved = 0.0;
254
255 for (unsigned int r = 0; r < j; r++) {
256 ndu[j][r] = right[r + 1] + left[j - r];
257 temp = ndu[r][j - 1] / ndu[j][r];
258 ndu[r][j] = saved + right[r + 1] * temp;
259 saved = left[j - r] * temp;
260 }
261 ndu[j][j] = saved;
262 }
263
264 for (unsigned int j = 0; j <= l_p; j++) {
265 N[0][j].value = ndu[j][l_p];
266 N[0][j].i = l_i - l_p + j;
267 N[0][j].p = l_p;
268 N[0][j].u = l_u;
269 N[0][j].k = 0;
270 }
271
272 if (l_der > l_p) {
273 vpTRACE("l_der must be under or equal to l_p");
274 l_der = l_p;
275 }
276
277 double d;
278 int rk;
279 unsigned int pk;
280 unsigned int j1, j2;
281
282 for (unsigned int r = 0; r <= l_p; r++) {
283 unsigned int s1 = 0;
284 unsigned int s2 = 1;
285 a[0][0] = 1.0;
286 for (unsigned int k = 1; k <= l_der; k++) {
287 d = 0.0;
288 rk = (int)(r - k);
289 pk = l_p - k;
290 if (r >= k) {
291 a[s2][0] = a[s1][0] / ndu[pk + 1][rk];
292 d = a[s2][0] * ndu[(unsigned int)rk][pk];
293 }
294
295 if (rk >= -1)
296 j1 = 1;
297 else
298 j1 = (unsigned int)(-rk);
299
300 if (r - 1 <= pk)
301 j2 = k - 1;
302 else
303 j2 = l_p - r;
304
305 for (unsigned int j = j1; j <= j2; j++) {
306 a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[pk + 1][(unsigned int)rk + j];
307 d += a[s2][j] * ndu[(unsigned int)rk + j][pk];
308 }
309
310 if (r <= pk) {
311 a[s2][k] = -a[s1][k - 1] / ndu[pk + 1][r];
312 d += a[s2][k] * ndu[r][pk];
313 }
314 N[k][r].value = d;
315 N[k][r].i = l_i - l_p + r;
316 N[k][r].p = l_p;
317 N[k][r].u = l_u;
318 N[k][r].k = k;
319
320 s1 = (s1 + 1) % 2;
321 s2 = (s2 + 1) % 2;
322 }
323 }
324
325 double r = l_p;
326 for (unsigned int k = 1; k <= l_der; k++) {
327 for (unsigned int j = 0; j <= l_p; j++)
328 N[k][j].value *= r;
329 r *= (l_p - k);
330 }
331
332 delete[] left;
333 delete[] right;
334
335 return N;
336 }
337
338 /*!
339 Compute the nonzero basis functions and their derivatives until the \f$ der
340 \f$ th derivative. All the functions are computed at u.
341
342 \warning \f$ der \f$ must be under or equal \f$ p \f$.
343
344 The result is given as an array of size der+1 x p+1. The kth line
345 corresponds to the kth basis functions derivatives.
346
347 The formula to compute the kth derivative at \f$ u \f$ is :
348
349 \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} -
350 \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
351
352 where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$
353 p \f$ is the degree of the B-Spline basis function.
354
355 \param u : A real number which is between the extrimities of the knot vector
356 \param der : The last derivative to be computed.
357
358 \return the basis functions and their derivatives as an array of size der+1
359 x p+1. The kth line corresponds to the kth basis functions derivatives.
360
361 Example : return[0] is the list of the 0th derivatives ie the basis
362 functions. return[k] is the list of the kth derivatives.
363 */
computeDersBasisFuns(double u,unsigned int der)364 vpBasisFunction **vpBSpline::computeDersBasisFuns(double u, unsigned int der)
365 {
366 unsigned int i = findSpan(u);
367 return computeDersBasisFuns(u, i, p, der, knots);
368 }
369
370 /*!
371 Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_i)
372 \f$ corresponding to the knot \f$ u \f$.
373
374 \param l_u : A real number which is between the extrimities of the knot
375 vector \param l_i : the number of the knot interval in which \f$ l_u \f$
376 lies \param l_p : Degree of the B-Spline basis functions. \param l_knots :
377 The knot vector \param l_controlPoints : the list of control points.
378
379 return the coordinates of a point corresponding to the knot \f$ u \f$.
380 */
computeCurvePoint(double l_u,unsigned int l_i,unsigned int l_p,std::vector<double> & l_knots,std::vector<vpImagePoint> & l_controlPoints)381 vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots,
382 std::vector<vpImagePoint> &l_controlPoints)
383 {
384 vpBasisFunction *N = computeBasisFuns(l_u, l_i, l_p, l_knots);
385 vpImagePoint pt;
386
387 double ic = 0;
388 double jc = 0;
389 for (unsigned int j = 0; j <= l_p; j++) {
390 ic = ic + N[j].value * (l_controlPoints[l_i - l_p + j]).get_i();
391 jc = jc + N[j].value * (l_controlPoints[l_i - l_p + j]).get_j();
392 }
393
394 pt.set_i(ic);
395 pt.set_j(jc);
396
397 delete[] N;
398
399 return pt;
400 }
401
402 /*!
403 Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_i)
404 \f$ corresponding to the knot \f$ u \f$.
405
406 \param u : A real number which is between the extrimities of the knot vector
407
408 return the coordinates of a point corresponding to the knot \f$ u \f$.
409 */
computeCurvePoint(double u)410 vpImagePoint vpBSpline::computeCurvePoint(double u)
411 {
412 vpBasisFunction *N = computeBasisFuns(u);
413 vpImagePoint pt;
414
415 double ic = 0;
416 double jc = 0;
417 for (unsigned int j = 0; j <= p; j++) {
418 ic = ic + N[j].value * (controlPoints[N[0].i + j]).get_i();
419 jc = jc + N[j].value * (controlPoints[N[0].i + j]).get_j();
420 }
421
422 pt.set_i(ic);
423 pt.set_j(jc);
424
425 delete[] N;
426
427 return pt;
428 }
429
430 /*!
431 Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der}
432 \f$.
433
434 The formula used is the following :
435
436 \f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)P_i) \f]
437
438 where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$
439 p \f$ is the degree of the B-Spline basis function.
440
441 \param l_u : A real number which is between the extrimities of the knot
442 vector \param l_i : the number of the knot interval in which \f$ l_u \f$
443 lies \param l_p : Degree of the B-Spline basis functions. \param l_der : The
444 last derivative to be computed. \param l_knots : The knot vector \param
445 l_controlPoints : the list of control points.
446
447 \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u)
448 \f$ for \f$ k = 0, ... , l_der \f$. The kth derivative is in the kth cell of
449 the array.
450 */
computeCurveDers(double l_u,unsigned int l_i,unsigned int l_p,unsigned int l_der,std::vector<double> & l_knots,std::vector<vpImagePoint> & l_controlPoints)451 vpImagePoint *vpBSpline::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
452 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
453 {
454 vpImagePoint *derivate = new vpImagePoint[l_der + 1];
455 vpBasisFunction **N;
456 N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
457
458 unsigned int du;
459 if (l_p < l_der) {
460 vpTRACE("l_der must be under or equal to l_p");
461 du = l_p;
462 } else
463 du = l_der;
464
465 for (unsigned int k = 0; k <= du; k++) {
466 derivate[k].set_ij(0.0, 0.0);
467 for (unsigned int j = 0; j <= l_p; j++) {
468 derivate[k].set_i(derivate[k].get_i() + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_i());
469 derivate[k].set_j(derivate[k].get_j() + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_j());
470 }
471 }
472
473 for (unsigned int j = 0; j <= l_der; j++)
474 delete[] N[j];
475 delete[] N;
476
477 return derivate;
478 }
479
480 /*!
481 Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , der \f$.
482
483 The formula used is the following :
484
485 \f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)P_i) \f]
486
487 where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$
488 p \f$ is the degree of the B-Spline basis function.
489
490 \param u : A real number which is between the extrimities of the knot vector
491 \param der : The last derivative to be computed.
492
493 \return an array of size der+1 containing the coordinates \f$ C^{(k)}(u) \f$
494 for \f$ k = 0, ... , der \f$. The kth derivative is in the kth cell of the
495 array.
496 */
computeCurveDers(double u,unsigned int der)497 vpImagePoint *vpBSpline::computeCurveDers(double u, unsigned int der)
498 {
499 vpImagePoint *derivate = new vpImagePoint[der + 1];
500 vpBasisFunction **N;
501 N = computeDersBasisFuns(u, der);
502
503 unsigned int du;
504 if (p < der) {
505 vpTRACE("der must be under or equal to p");
506 du = p;
507 } else
508 du = der;
509
510 for (unsigned int k = 0; k <= du; k++) {
511 derivate[k].set_ij(0.0, 0.0);
512 for (unsigned int j = 0; j <= p; j++) {
513 derivate[k].set_i(derivate[k].get_i() + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_i());
514 derivate[k].set_j(derivate[k].get_j() + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_j());
515 }
516 }
517
518 for (unsigned int j = 0; j <= der; j++)
519 delete[] N[j];
520 delete[] N;
521
522 return derivate;
523 }
524