Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
vpBSpline.cpp
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 
49  : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
50  crossingPoints()
51 {
52 }
53 
59  : controlPoints(bspline.controlPoints), knots(bspline.knots), p(bspline.p), // By default : p=3 for clubic spline
60  crossingPoints(bspline.crossingPoints)
61 {
62 }
67 
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 
129 unsigned int vpBSpline::findSpan(double u) { return findSpan(u, p, knots); }
130 
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 
199 vpBasisFunction *vpBSpline::computeBasisFuns(double u)
200 {
201  unsigned int i = findSpan(u);
202  return computeBasisFuns(u, i, p, knots);
203 }
204 
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 
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 
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 
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 
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 
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 }
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)
Definition: vpBSpline.cpp:451
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
virtual ~vpBSpline()
Definition: vpBSpline.cpp:66
static vpBasisFunction ** computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector< double > &l_knots)
Definition: vpBSpline.cpp:234
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:143
static unsigned int findSpan(double l_u, unsigned int l_p, std::vector< double > &l_knots)
Definition: vpBSpline.cpp:84
void set_i(double ii)
Definition: vpImagePoint.h:167
#define vpTRACE
Definition: vpDebug.h:416
static vpBasisFunction * computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots)
Definition: vpBSpline.cpp:148
static int round(double x)
Definition: vpMath.h:241
void set_j(double jj)
Definition: vpImagePoint.h:178
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:189
Class that provides tools to compute and manipulate a B-Spline curve.
Definition: vpBSpline.h:110
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints)
Definition: vpBSpline.cpp:381