Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpBSpline.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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*****************************************************************************/
35
36#include <visp3/core/vpBSpline.h>
37#include <visp3/core/vpDebug.h>
38
46 : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
47 crossingPoints()
48{ }
49
55 : controlPoints(bspline.controlPoints), knots(bspline.knots), p(bspline.p), // By default : p=3 for clubic spline
56 crossingPoints(bspline.crossingPoints)
57{ }
62
79unsigned int vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots)
80{
81 unsigned int m = (unsigned int)l_knots.size() - 1;
82
83 if (l_u > l_knots.back()) {
84 // vpTRACE("l_u higher than the maximum value in the knot vector :
85 // %lf",l_u);
86 return ((unsigned int)(m - l_p - 1));
87 }
88
89 // if (l_u == l_knots.back())
90 if (std::fabs(l_u - l_knots.back()) <=
91 std::fabs(vpMath::maximum(l_u, l_knots.back())) * std::numeric_limits<double>::epsilon())
92 return ((unsigned int)(m - l_p - 1));
93
94 double low = l_p;
95 double high = m - l_p;
96 double middle = (low + high) / 2.0;
97
98 while (l_u < l_knots[(unsigned int)middle] || l_u >= l_knots[(unsigned int)middle + 1]) {
99 if (l_u < l_knots[(unsigned int)vpMath::round(middle)])
100 high = middle;
101 else
102 low = middle;
103 middle = (low + high) / 2.0;
104 }
105
106 return (unsigned int)middle;
107}
108
123unsigned int vpBSpline::findSpan(double u) { return findSpan(u, p, knots); }
124
142vpBasisFunction *vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p,
143 std::vector<double> &l_knots)
144{
145 vpBasisFunction *N = new vpBasisFunction[l_p + 1];
146
147 N[0].value = 1.0;
148
149 double *left = new double[l_p + 1];
150 double *right = new double[l_p + 1];
151 double temp = 0.0;
152
153 for (unsigned int j = 1; j <= l_p; j++) {
154 left[j] = l_u - l_knots[l_i + 1 - j];
155 right[j] = l_knots[l_i + j] - l_u;
156 double saved = 0.0;
157
158 for (unsigned int r = 0; r < j; r++) {
159 temp = N[r].value / (right[r + 1] + left[j - r]);
160 N[r].value = saved + right[r + 1] * temp;
161 saved = left[j - r] * temp;
162 }
163 N[j].value = saved;
164 }
165 for (unsigned int j = 0; j < l_p + 1; j++) {
166 N[j].i = l_i - l_p + j;
167 N[j].p = l_p;
168 N[j].u = l_u;
169 N[j].k = 0;
170 }
171
172 delete[] left;
173 delete[] right;
174
175 return N;
176}
177
193vpBasisFunction *vpBSpline::computeBasisFuns(double u)
194{
195 unsigned int i = findSpan(u);
196 return computeBasisFuns(u, i, p, knots);
197}
198
228vpBasisFunction **vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
229 std::vector<double> &l_knots)
230{
231 vpBasisFunction **N;
232 N = new vpBasisFunction *[l_der + 1];
233 for (unsigned int j = 0; j <= l_der; j++)
234 N[j] = new vpBasisFunction[l_p + 1];
235
236 vpMatrix a(2, l_p + 1);
237 vpMatrix ndu(l_p + 1, l_p + 1);
238 ndu[0][0] = 1.0;
239
240 double *left = new double[l_p + 1];
241 double *right = new double[l_p + 1];
242 double temp = 0.0;
243
244 for (unsigned int j = 1; j <= l_p; j++) {
245 left[j] = l_u - l_knots[l_i + 1 - j];
246 right[j] = l_knots[l_i + j] - l_u;
247 double saved = 0.0;
248
249 for (unsigned int r = 0; r < j; r++) {
250 ndu[j][r] = right[r + 1] + left[j - r];
251 temp = ndu[r][j - 1] / ndu[j][r];
252 ndu[r][j] = saved + right[r + 1] * temp;
253 saved = left[j - r] * temp;
254 }
255 ndu[j][j] = saved;
256 }
257
258 for (unsigned int j = 0; j <= l_p; j++) {
259 N[0][j].value = ndu[j][l_p];
260 N[0][j].i = l_i - l_p + j;
261 N[0][j].p = l_p;
262 N[0][j].u = l_u;
263 N[0][j].k = 0;
264 }
265
266 if (l_der > l_p) {
267 vpTRACE("l_der must be under or equal to l_p");
268 l_der = l_p;
269 }
270
271 double d;
272 int rk;
273 unsigned int pk;
274 unsigned int j1, j2;
275
276 for (unsigned int r = 0; r <= l_p; r++) {
277 unsigned int s1 = 0;
278 unsigned int s2 = 1;
279 a[0][0] = 1.0;
280 for (unsigned int k = 1; k <= l_der; k++) {
281 d = 0.0;
282 rk = (int)(r - k);
283 pk = l_p - k;
284 if (r >= k) {
285 a[s2][0] = a[s1][0] / ndu[pk + 1][rk];
286 d = a[s2][0] * ndu[(unsigned int)rk][pk];
287 }
288
289 if (rk >= -1)
290 j1 = 1;
291 else
292 j1 = (unsigned int)(-rk);
293
294 if (r - 1 <= pk)
295 j2 = k - 1;
296 else
297 j2 = l_p - r;
298
299 for (unsigned int j = j1; j <= j2; j++) {
300 a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[pk + 1][(unsigned int)rk + j];
301 d += a[s2][j] * ndu[(unsigned int)rk + j][pk];
302 }
303
304 if (r <= pk) {
305 a[s2][k] = -a[s1][k - 1] / ndu[pk + 1][r];
306 d += a[s2][k] * ndu[r][pk];
307 }
308 N[k][r].value = d;
309 N[k][r].i = l_i - l_p + r;
310 N[k][r].p = l_p;
311 N[k][r].u = l_u;
312 N[k][r].k = k;
313
314 s1 = (s1 + 1) % 2;
315 s2 = (s2 + 1) % 2;
316 }
317 }
318
319 double r = l_p;
320 for (unsigned int k = 1; k <= l_der; k++) {
321 for (unsigned int j = 0; j <= l_p; j++)
322 N[k][j].value *= r;
323 r *= (l_p - k);
324 }
325
326 delete[] left;
327 delete[] right;
328
329 return N;
330}
331
358vpBasisFunction **vpBSpline::computeDersBasisFuns(double u, unsigned int der)
359{
360 unsigned int i = findSpan(u);
361 return computeDersBasisFuns(u, i, p, der, knots);
362}
363
375vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots,
376 std::vector<vpImagePoint> &l_controlPoints)
377{
378 vpBasisFunction *N = computeBasisFuns(l_u, l_i, l_p, l_knots);
379 vpImagePoint pt;
380
381 double ic = 0;
382 double jc = 0;
383 for (unsigned int j = 0; j <= l_p; j++) {
384 ic = ic + N[j].value * (l_controlPoints[l_i - l_p + j]).get_i();
385 jc = jc + N[j].value * (l_controlPoints[l_i - l_p + j]).get_j();
386 }
387
388 pt.set_i(ic);
389 pt.set_j(jc);
390
391 delete[] N;
392
393 return pt;
394}
395
405{
406 vpBasisFunction *N = computeBasisFuns(u);
407 vpImagePoint pt;
408
409 double ic = 0;
410 double jc = 0;
411 for (unsigned int j = 0; j <= p; j++) {
412 ic = ic + N[j].value * (controlPoints[N[0].i + j]).get_i();
413 jc = jc + N[j].value * (controlPoints[N[0].i + j]).get_j();
414 }
415
416 pt.set_i(ic);
417 pt.set_j(jc);
418
419 delete[] N;
420
421 return pt;
422}
423
445vpImagePoint *vpBSpline::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
446 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
447{
448 vpImagePoint *derivate = new vpImagePoint[l_der + 1];
449 vpBasisFunction **N;
450 N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
451
452 unsigned int du;
453 if (l_p < l_der) {
454 vpTRACE("l_der must be under or equal to l_p");
455 du = l_p;
456 }
457 else
458 du = l_der;
459
460 for (unsigned int k = 0; k <= du; k++) {
461 derivate[k].set_ij(0.0, 0.0);
462 for (unsigned int j = 0; j <= l_p; j++) {
463 derivate[k].set_i(derivate[k].get_i() + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_i());
464 derivate[k].set_j(derivate[k].get_j() + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_j());
465 }
466 }
467
468 for (unsigned int j = 0; j <= l_der; j++)
469 delete[] N[j];
470 delete[] N;
471
472 return derivate;
473}
474
492vpImagePoint *vpBSpline::computeCurveDers(double u, unsigned int der)
493{
494 vpImagePoint *derivate = new vpImagePoint[der + 1];
495 vpBasisFunction **N;
496 N = computeDersBasisFuns(u, der);
497
498 unsigned int du;
499 if (p < der) {
500 vpTRACE("der must be under or equal to p");
501 du = p;
502 }
503 else
504 du = der;
505
506 for (unsigned int k = 0; k <= du; k++) {
507 derivate[k].set_ij(0.0, 0.0);
508 for (unsigned int j = 0; j <= p; j++) {
509 derivate[k].set_i(derivate[k].get_i() + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_i());
510 derivate[k].set_j(derivate[k].get_j() + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_j());
511 }
512 }
513
514 for (unsigned int j = 0; j <= der; j++)
515 delete[] N[j];
516 delete[] N;
517
518 return derivate;
519}
Class that provides tools to compute and manipulate a B-Spline curve.
Definition vpBSpline.h:106
static vpBasisFunction ** computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector< double > &l_knots)
static vpBasisFunction * computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots)
unsigned int p
Degree of the B-Spline basis functions.
Definition vpBSpline.h:113
virtual ~vpBSpline()
Definition vpBSpline.cpp:61
static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints)
static vpImagePoint * 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)
std::vector< double > knots
Vector which contain the knots .
Definition vpBSpline.h:111
static unsigned int findSpan(double l_u, unsigned int l_p, std::vector< double > &l_knots)
Definition vpBSpline.cpp:79
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_j(double jj)
void set_ij(double ii, double jj)
void set_i(double ii)
static Type maximum(const Type &a, const Type &b)
Definition vpMath.h:172
static int round(double x)
Definition vpMath.h:323
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
#define vpTRACE
Definition vpDebug.h:411