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