ViSP  2.10.0
vpBSpline.cpp
1 /****************************************************************************
2  *
3  * $Id: vpBSpline.cpp 4632 2014-02-03 17:06:40Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * This class implements the B-Spline
36  *
37  * Authors:
38  * Nicolas Melchior
39  *
40  *****************************************************************************/
41 
42 #include <visp/vpBSpline.h>
43 #include <visp/vpDebug.h>
44 
52  : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
53  crossingPoints()
54 {
55 }
56 
62  : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
63  crossingPoints()
64 {
65  controlPoints = bspline.controlPoints;
66  knots = bspline.knots;
67  p = bspline.p;
68  crossingPoints = bspline.crossingPoints;
69 }
74 {
75 }
76 
91 unsigned int
92 vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots)
93 {
94  unsigned int m = (unsigned int)l_knots.size()-1;
95 
96  if(l_u > l_knots.back())
97  {
98  //vpTRACE("l_u higher than the maximum value in the knot vector : %lf",l_u);
99  return((unsigned int)(m-l_p-1));
100  }
101 
102  //if (l_u == l_knots.back())
103  if (std::fabs(l_u - l_knots.back()) <= std::fabs(vpMath::maximum(l_u, l_knots.back())) * std::numeric_limits<double>::epsilon())
104  return((unsigned int)(m-l_p-1));
105 
106  double low = l_p;
107  double high = m-l_p;
108  double middle = (low+high)/2.0;
109 
110  while (l_u < l_knots[(unsigned int)vpMath::round(middle)] || l_u >= l_knots[(unsigned int)vpMath::round(middle+1)])
111  {
112  if(l_u < l_knots[(unsigned int)vpMath::round(middle)]) high = middle;
113  else low = middle;
114  middle = (low+high)/2.0;
115  }
116 
117  return (unsigned int)middle;
118 }
119 
120 
133 unsigned int
135 {
136  return findSpan( u, p, knots);
137 }
138 
139 
152 vpBasisFunction* vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots)
153 {
154  vpBasisFunction* N = new vpBasisFunction[l_p+1];
155 
156  N[0].value = 1.0;
157 
158  double *left = new double[l_p+1];
159  double *right = new double[l_p+1];
160  double saved = 0.0;
161  double temp = 0.0;
162 
163  for(unsigned int j = 1; j <= l_p; j++)
164  {
165  left[j] = l_u - l_knots[l_i+1-j];
166  right[j] = l_knots[l_i+j] - l_u;
167  saved = 0.0;
168 
169  for (unsigned int r = 0; r < j; r++)
170  {
171  temp = N[r].value / (right[r+1]+left[j-r]);
172  N[r].value = saved +right[r+1]*temp;
173  saved = left[j-r]*temp;
174  }
175  N[j].value = saved;
176  }
177  for(unsigned int j = 0; j < l_p+1; j++)
178  {
179  N[j].i = l_i-l_p+j;
180  N[j].p = l_p;
181  N[j].u = l_u;
182  N[j].k = 0;
183  }
184 
185  delete[] left;
186  delete[] right;
187 
188  return N;
189 }
190 
191 
203 vpBasisFunction* vpBSpline::computeBasisFuns(double u)
204 {
205  unsigned int i = findSpan(u);
206  return computeBasisFuns(u, i, p ,knots);
207 }
208 
209 
233 vpBasisFunction** vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots)
234 {
235  vpBasisFunction** N;
236  N = new vpBasisFunction*[l_der+1];
237  for(unsigned int j = 0; j <= l_der; j++)
238  N[j] = new vpBasisFunction[l_p+1];
239 
240  vpMatrix a(2,l_p+1);
241  vpMatrix ndu(l_p+1,l_p+1);
242  ndu[0][0] = 1.0;
243 
244  double *left = new double[l_p+1];
245  double *right = new double[l_p+1];
246  double saved = 0.0;
247  double temp = 0.0;
248 
249  for(unsigned int j = 1; j <= l_p; j++)
250  {
251  left[j] = l_u - l_knots[l_i+1-j];
252  right[j] = l_knots[l_i+j] - l_u;
253  saved = 0.0;
254 
255  for (unsigned int r = 0; r < j; r++)
256  {
257  ndu[j][r] = right[r+1]+left[j-r];
258  temp = ndu[r][j-1]/ndu[j][r];
259  ndu[r][j] = saved + right[r+1]*temp;
260  saved = left[j-r]*temp;
261  }
262  ndu[j][j] = saved;
263  }
264 
265  for(unsigned int j = 0; j <= l_p; j++)
266  {
267  N[0][j].value = ndu[j][l_p];
268  N[0][j].i = l_i-l_p+j;
269  N[0][j].p = l_p;
270  N[0][j].u = l_u;
271  N[0][j].k = 0;
272  }
273 
274  if( l_der > l_p)
275  {
276  vpTRACE("l_der must be under or equal to l_p");
277  l_der = l_p;
278  }
279 
280  unsigned int s1,s2;
281  double d;
282  int rk;
283  unsigned int pk;
284  unsigned int j1,j2;
285 
286  for (unsigned int r = 0; r <= l_p; r++)
287  {
288  s1 = 0;
289  s2 = 1;
290  a[0][0] = 1.0;
291  for(unsigned int k = 1; k <= l_der; k++)
292  {
293  d = 0.0;
294  rk = (int)(r-k);
295  pk = l_p-k;
296  if(r >= k)
297  {
298  a[s2][0] = a[s1][0]/ndu[pk+1][rk];
299  d = a[s2][0]*ndu[(unsigned int)rk][pk];
300  }
301 
302  if(rk >= -1)
303  j1 = 1;
304  else
305  j1 = (unsigned int)(-rk);
306 
307  if(r-1 <= pk)
308  j2 = k-1;
309  else
310  j2 = l_p-r;
311 
312  for(unsigned int j =j1; j<= j2; j++)
313  {
314  a[s2][j] = (a[s1][j]-a[s1][j-1])/ndu[pk+1][(unsigned int)rk+j];
315  d += a[s2][j]*ndu[(unsigned int)rk+j][pk];
316  }
317 
318  if(r <= pk)
319  {
320  a[s2][k] = -a[s1][k-1]/ndu[pk+1][r];
321  d += a[s2][k]*ndu[r][pk];
322  }
323  N[k][r].value = d;
324  N[k][r].i = l_i-l_p+r;
325  N[k][r].p = l_p;
326  N[k][r].u = l_u;
327  N[k][r].k = k;
328 
329  s1 = (s1+1)%2;
330  s2 = (s2+1)%2;
331  }
332  }
333 
334  double r = l_p;
335  for ( unsigned int k = 1; k <= l_der; k++ )
336  {
337  for (unsigned int j = 0; j <= l_p; j++)
338  N[k][j].value *= r;
339  r *= (l_p-k);
340  }
341 
342  delete[] left;
343  delete[] right;
344 
345  return N;
346 }
347 
348 
369 vpBasisFunction** vpBSpline::computeDersBasisFuns(double u, unsigned int der)
370 {
371  unsigned int i = findSpan(u);
372  return computeDersBasisFuns(u, i, p , der, knots);
373 }
374 
375 
387 vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
388 {
389  vpBasisFunction* N = computeBasisFuns(l_u, l_i, l_p, l_knots);
390  vpImagePoint pt;
391 
392  double ic = 0;
393  double jc = 0;
394  for(unsigned int j = 0; j <= l_p; j++)
395  {
396  ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i();
397  jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j();
398  }
399 
400  pt.set_i(ic);
401  pt.set_j(jc);
402 
403  delete[] N;
404 
405  return pt;
406 }
407 
408 
417 {
418  vpBasisFunction* N = computeBasisFuns(u);
419  vpImagePoint pt;
420 
421  double ic = 0;
422  double jc = 0;
423  for(unsigned int j = 0; j <= p; j++)
424  {
425  ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i();
426  jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j();
427  }
428 
429  pt.set_i(ic);
430  pt.set_j(jc);
431 
432  delete[] N;
433 
434  return pt;
435 }
436 
437 
456 vpImagePoint* vpBSpline::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)
457 {
458  vpImagePoint *derivate = new vpImagePoint[l_der+1];
459  vpBasisFunction** N;
460  N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
461 
462  unsigned int du;
463  if (l_p < l_der)
464  {
465  vpTRACE("l_der must be under or equal to l_p");
466  du = l_p;
467  }
468  else du = l_der;
469 
470  for(unsigned int k = 0; k <= du; k++)
471  {
472  derivate[k].set_ij(0.0,0.0);
473  for(unsigned int j = 0; j<= l_p; j++)
474  {
475  derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i());
476  derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j());
477  }
478  }
479 
480 
481  for(unsigned int j = 0; j <= l_der; j++)
482  delete[] N[j];
483  delete[] N;
484 
485 
486  return derivate;
487 }
488 
489 
504 vpImagePoint* vpBSpline::computeCurveDers(double u, unsigned int der)
505 {
506  vpImagePoint *derivate = new vpImagePoint[der+1];
507  vpBasisFunction** N;
508  N = computeDersBasisFuns(u, der);
509 
510  unsigned int du;
511  if (p < der)
512  {
513  vpTRACE("der must be under or equal to p");
514  du = p;
515  }
516  else du = der;
517 
518  for(unsigned int k = 0; k <= du; k++)
519  {
520  derivate[k].set_ij(0.0,0.0);
521  for(unsigned int j = 0; j<= p; j++)
522  {
523  derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i());
524  derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j());
525  }
526  }
527 
528  for(unsigned int j = 0; j <= der; j++)
529  delete[] N[j];
530  delete[] N;
531 
532  return derivate;
533 }
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:456
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
virtual ~vpBSpline()
Definition: vpBSpline.cpp:73
#define vpTRACE
Definition: vpDebug.h:418
static int round(const double x)
Definition: vpMath.h:228
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:233
static Type maximum(const Type &a, const Type &b)
Definition: vpMath.h:137
static unsigned int findSpan(double l_u, unsigned int l_p, std::vector< double > &l_knots)
Definition: vpBSpline.cpp:92
void set_i(const double ii)
Definition: vpImagePoint.h:159
static vpBasisFunction * computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots)
Definition: vpBSpline.cpp:152
void set_j(const double jj)
Definition: vpImagePoint.h:170
Class that provides tools to compute and manipulate a B-Spline curve.
Definition: vpBSpline.h:109
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:93
void set_ij(const double ii, const double jj)
Definition: vpImagePoint.h:181
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:387