Visual Servoing Platform  version 3.5.1 under development (2023-05-30)
vpNurbs.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 Non Uniform Rational B-Spline (NURBS)
33  *
34  *****************************************************************************/
35 
36 #include <cmath> // std::fabs
37 #include <limits> // numeric_limits
38 #include <visp3/core/vpColVector.h>
39 #include <visp3/me/vpNurbs.h>
40 /*
41  Compute the distance d = |Pw1-Pw2|
42 */
43 inline double distance(const vpImagePoint &iP1, double w1, const vpImagePoint &iP2, double w2)
44 {
45  double distancei = iP1.get_i() - iP2.get_i();
46  double distancej = iP1.get_j() - iP2.get_j();
47  double distancew = w1 - w2;
48  return sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew));
49 }
50 
57 vpNurbs::vpNurbs() : weights() { p = 3; }
58 
62 vpNurbs::vpNurbs(const vpNurbs &nurbs) : vpBSpline(nurbs), weights(nurbs.weights) {}
63 
68 
82 vpImagePoint vpNurbs::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots,
83  std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
84 {
85  vpBasisFunction *N = NULL;
86  N = computeBasisFuns(l_u, l_i, l_p, l_knots);
87  vpImagePoint pt;
88 
89  double ic = 0;
90  double jc = 0;
91  double wc = 0;
92  for (unsigned int j = 0; j <= l_p; j++) {
93  ic = ic + N[j].value * (l_controlPoints[l_i - l_p + j]).get_i() * l_weights[l_i - l_p + j];
94  jc = jc + N[j].value * (l_controlPoints[l_i - l_p + j]).get_j() * l_weights[l_i - l_p + j];
95  wc = wc + N[j].value * l_weights[l_i - l_p + j];
96  }
97 
98  pt.set_i(ic / wc);
99  pt.set_j(jc / wc);
100 
101  if (N != NULL)
102  delete[] N;
103 
104  return pt;
105 }
106 
117 {
118  vpBasisFunction *N = NULL;
119  N = computeBasisFuns(u);
120  vpImagePoint pt;
121 
122  double ic = 0;
123  double jc = 0;
124  double wc = 0;
125  for (unsigned int j = 0; j <= p; j++) {
126  ic = ic + N[j].value * (controlPoints[N[0].i + j]).get_i() * weights[N[0].i + j]; // N[0].i = findSpan(u)-p
127  jc = jc + N[j].value * (controlPoints[N[0].i + j]).get_j() * weights[N[0].i + j];
128  wc = wc + N[j].value * weights[N[0].i + j];
129  }
130 
131  pt.set_i(ic / wc);
132  pt.set_j(jc / wc);
133 
134  if (N != NULL)
135  delete[] N;
136 
137  return pt;
138 }
139 
167 vpMatrix vpNurbs::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
168  std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
169  std::vector<double> &l_weights)
170 {
171  vpMatrix derivate(l_der + 1, 3);
172  vpBasisFunction **N = NULL;
173  N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
174 
175  for (unsigned int k = 0; k <= l_der; k++) {
176  derivate[k][0] = 0.0;
177  derivate[k][1] = 0.0;
178  derivate[k][2] = 0.0;
179 
180  for (unsigned int j = 0; j <= l_p; j++) {
181  derivate[k][0] = derivate[k][0] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_i();
182  derivate[k][1] = derivate[k][1] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_j();
183  derivate[k][2] = derivate[k][2] + N[k][j].value * (l_weights[l_i - l_p + j]);
184  }
185  }
186 
187  if (N != NULL) {
188  for (unsigned int i = 0; i <= l_der; i++)
189  delete[] N[i];
190  delete[] N;
191  }
192  return derivate;
193 }
194 
217 vpMatrix vpNurbs::computeCurveDers(double u, unsigned int der)
218 {
219  vpMatrix derivate(der + 1, 3);
220  vpBasisFunction **N = NULL;
221  N = computeDersBasisFuns(u, der);
222 
223  for (unsigned int k = 0; k <= der; k++) {
224  derivate[k][0] = 0.0;
225  derivate[k][1] = 0.0;
226  derivate[k][2] = 0.0;
227  for (unsigned int j = 0; j <= p; j++) {
228  derivate[k][0] = derivate[k][0] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_i();
229  derivate[k][1] = derivate[k][1] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_j();
230  derivate[k][2] = derivate[k][2] + N[k][j].value * (weights[N[0][0].i - p + j]);
231  }
232  }
233 
234  if (N != NULL)
235  delete[] N;
236 
237  return derivate;
238 }
239 
257 vpImagePoint *vpNurbs::computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
258  std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
259  std::vector<double> &l_weights)
260 {
261  std::vector<vpImagePoint> A;
262  vpImagePoint pt;
263  for (unsigned int j = 0; j < l_controlPoints.size(); j++) {
264  pt = l_controlPoints[j];
265  pt.set_i(pt.get_i() * l_weights[j]);
266  pt.set_j(pt.get_j() * l_weights[j]);
267  A.push_back(pt);
268  }
269 
270  vpMatrix Awders = computeCurveDers(l_u, l_i, l_p, l_der, l_knots, A, l_weights);
271 
272  vpImagePoint *CK = new vpImagePoint[l_der + 1];
273 
274  for (unsigned int k = 0; k <= l_der; k++) {
275  double ic = Awders[k][0];
276  double jc = Awders[k][1];
277  for (unsigned int j = 1; j <= k; j++) {
278  double tmpComb = static_cast<double>(vpMath::comb(k, j));
279  ic = ic - tmpComb * Awders[k][2] * (CK[k - j].get_i());
280  jc = jc - tmpComb * Awders[j][2] * (CK[k - j].get_j());
281  }
282  CK[k].set_ij(ic / Awders[0][2], jc / Awders[0][2]);
283  }
284  return CK;
285 }
286 
300 vpImagePoint *vpNurbs::computeCurveDersPoint(double u, unsigned int der)
301 {
302  unsigned int i = findSpan(u);
303  return computeCurveDersPoint(u, i, p, der, knots, controlPoints, weights);
304 }
305 
322 void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p,
323  std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
324  std::vector<double> &l_weights)
325 {
326  vpMatrix Rw(l_p + 1, 3);
327  std::vector<vpImagePoint>::iterator it1;
328  std::vector<double>::iterator it2;
329  vpImagePoint pt;
330  double w = 0;
331 
332  for (unsigned int j = 0; j <= l_p - l_s; j++) {
333  Rw[j][0] = (l_controlPoints[l_k - l_p + j]).get_i() * l_weights[l_k - l_p + j];
334  Rw[j][1] = (l_controlPoints[l_k - l_p + j]).get_j() * l_weights[l_k - l_p + j];
335  Rw[j][2] = l_weights[l_k - l_p + j];
336  }
337 
338  it1 = l_controlPoints.begin();
339  l_controlPoints.insert(it1 + (int)l_k - (int)l_s, l_r, pt);
340  it2 = l_weights.begin();
341  l_weights.insert(it2 + (int)l_k - (int)l_s, l_r, w);
342 
343  unsigned int L = 0;
344  double alpha;
345  for (unsigned int j = 1; j <= l_r; j++) {
346  L = l_k - l_p + j;
347 
348  for (unsigned int i = 0; i <= l_p - j - l_s; i++) {
349  alpha = (l_u - l_knots[L + i]) / (l_knots[i + l_k + 1] - l_knots[L + i]);
350  Rw[i][0] = alpha * Rw[i + 1][0] + (1.0 - alpha) * Rw[i][0];
351  Rw[i][1] = alpha * Rw[i + 1][1] + (1.0 - alpha) * Rw[i][1];
352  Rw[i][2] = alpha * Rw[i + 1][2] + (1.0 - alpha) * Rw[i][2];
353  }
354 
355  pt.set_ij(Rw[0][0] / Rw[0][2], Rw[0][1] / Rw[0][2]);
356  l_controlPoints[L] = pt;
357  l_weights[L] = Rw[0][2];
358 
359  pt.set_ij(Rw[l_p - j - l_s][0] / Rw[l_p - j - l_s][2], Rw[l_p - j - l_s][1] / Rw[l_p - j - l_s][2]);
360  l_controlPoints[l_k + l_r - j - l_s] = pt;
361  l_weights[l_k + l_r - j - l_s] = Rw[l_p - j - l_s][2];
362  }
363 
364  for (unsigned int j = L + 1; j < l_k - l_s; j++) {
365  pt.set_ij(Rw[j - L][0] / Rw[j - L][2], Rw[j - L][1] / Rw[j - L][2]);
366  l_controlPoints[j] = pt;
367  l_weights[j] = Rw[j - L][2];
368  }
369 
370  it2 = l_knots.begin();
371  l_knots.insert(it2 + (int)l_k, l_r, l_u);
372 }
373 
385 void vpNurbs::curveKnotIns(double u, unsigned int s, unsigned int r)
386 {
387  unsigned int i = findSpan(u);
388  curveKnotIns(u, i, s, r, p, knots, controlPoints, weights);
389 }
390 
404 void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots,
405  std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
406 {
407  unsigned int a = findSpan(l_x[0], l_p, l_knots);
408  unsigned int b = findSpan(l_x[l_r], l_p, l_knots);
409  b++;
410 
411  unsigned int n = (unsigned int)l_controlPoints.size();
412  unsigned int m = (unsigned int)l_knots.size();
413 
414  for (unsigned int j = 0; j < n; j++) {
415  l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
416  }
417 
418  std::vector<double> l_knots_tmp(l_knots);
419  std::vector<vpImagePoint> l_controlPoints_tmp(l_controlPoints);
420  std::vector<double> l_weights_tmp(l_weights);
421 
422  vpImagePoint pt;
423  double w = 0;
424 
425  for (unsigned int j = 0; j <= l_r; j++) {
426  l_controlPoints.push_back(pt);
427  l_weights.push_back(w);
428  l_knots.push_back(w);
429  }
430 
431  for (unsigned int j = b + l_p; j <= m - 1; j++)
432  l_knots[j + l_r + 1] = l_knots_tmp[j];
433 
434  for (unsigned int j = b - 1; j <= n - 1; j++) {
435  l_controlPoints[j + l_r + 1] = l_controlPoints_tmp[j];
436  l_weights[j + l_r + 1] = l_weights_tmp[j];
437  }
438 
439  unsigned int i = b + l_p - 1;
440  unsigned int k = b + l_p + l_r;
441 
442  {
443  unsigned int j = l_r + 1;
444  do {
445  j--;
446  while (l_x[j] <= l_knots[i] && i > a) {
447  l_controlPoints[k - l_p - 1] = l_controlPoints_tmp[i - l_p - 1];
448  l_weights[k - l_p - 1] = l_weights_tmp[i - l_p - 1];
449  l_knots[k] = l_knots_tmp[i];
450  k--;
451  i--;
452  }
453 
454  l_controlPoints[k - l_p - 1] = l_controlPoints[k - l_p];
455  l_weights[k - l_p - 1] = l_weights[k - l_p];
456 
457  for (unsigned int l = 1; l <= l_p; l++) {
458  unsigned int ind = k - l_p + l;
459  double alpha = l_knots[k + l] - l_x[j];
460  // if (vpMath::abs(alpha) == 0.0)
461  if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon()) {
462  l_controlPoints[ind - 1] = l_controlPoints[ind];
463  l_weights[ind - 1] = l_weights[ind];
464  } else {
465  alpha = alpha / (l_knots[k + l] - l_knots_tmp[i - l_p + l]);
466  l_controlPoints[ind - 1].set_i(alpha * l_controlPoints[ind - 1].get_i() +
467  (1.0 - alpha) * l_controlPoints[ind].get_i());
468  l_controlPoints[ind - 1].set_j(alpha * l_controlPoints[ind - 1].get_j() +
469  (1.0 - alpha) * l_controlPoints[ind].get_j());
470  l_weights[ind - 1] = alpha * l_weights[ind - 1] + (1.0 - alpha) * l_weights[ind];
471  }
472  }
473  l_knots[k] = l_x[j];
474  k--;
475  } while (j != 0);
476  }
477 
478  for (unsigned int j = 0; j < n; j++) {
479  l_controlPoints[j].set_ij(l_controlPoints[j].get_i() / l_weights[j], l_controlPoints[j].get_j() / l_weights[j]);
480  }
481 }
482 
493 void vpNurbs::refineKnotVectCurve(double *x, unsigned int r)
494 {
495  refineKnotVectCurve(x, r, p, knots, controlPoints, weights);
496 }
497 
523 unsigned int vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s,
524  unsigned int l_p, std::vector<double> &l_knots,
525  std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
526 {
527  unsigned int n = (unsigned int)l_controlPoints.size();
528  unsigned int m = n + l_p + 1;
529 
530  for (unsigned int j = 0; j < n; j++) {
531  l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
532  }
533 
534  unsigned int ord = l_p + 1;
535  double fout = (2 * l_r - l_s - l_p) / 2.;
536  unsigned int last = l_r - l_s;
537  unsigned int first = l_r - l_p;
538  unsigned int tblSize = 2 * l_p + 1;
539  vpImagePoint *tempP = new vpImagePoint[tblSize];
540  double *tempW = new double[tblSize];
541  vpImagePoint pt;
542  unsigned int t = 0;
543  double alfi = 0;
544  double alfj = 0;
545  unsigned int i, j;
546 
547  for (t = 0; t < l_num; t++) {
548  unsigned int off = first - 1;
549  tempP[0] = l_controlPoints[off];
550  tempW[0] = l_weights[off];
551  tempP[last + 1 - off] = l_controlPoints[last + 1];
552  tempW[last + 1 - off] = l_weights[last + 1];
553  i = first;
554  j = last;
555  unsigned int ii = 1;
556  unsigned int jj = last - off;
557  int remflag = 0;
558  while (j - i > t) {
559  alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
560  alfj = (l_u - l_knots[j - t]) / (l_knots[j + ord] - l_knots[j - t]);
561  pt.set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
562  tempP[ii].set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
563  tempP[ii].set_j((l_controlPoints[i].get_j() - (1.0 - alfi) * tempP[ii - 1].get_j()) / alfi);
564  tempW[ii] = ((l_weights[i] - (1.0 - alfi) * tempW[ii - 1]) / alfi);
565  tempP[jj].set_i((l_controlPoints[j].get_i() - alfj * tempP[jj + 1].get_i()) / (1.0 - alfj));
566  tempP[jj].set_j((l_controlPoints[j].get_j() - alfj * tempP[jj + 1].get_j()) / (1.0 - alfj));
567  tempW[jj] = ((l_weights[j] - alfj * tempW[jj + 1]) / (1.0 - alfj));
568  i++;
569  j--;
570  ii++;
571  jj--;
572  }
573 
574  if (j - i < t) {
575  double distancei = tempP[ii - 1].get_i() - tempP[jj + 1].get_i();
576  double distancej = tempP[ii - 1].get_j() - tempP[jj + 1].get_j();
577  double distancew = tempW[ii - 1] - tempW[jj + 1];
578  double distance = sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew));
579  if (distance <= l_TOL)
580  remflag = 1;
581  } else {
582  alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
583  double distancei =
584  l_controlPoints[i].get_i() - (alfi * tempP[ii + t + 1].get_i() + (1.0 - alfi) * tempP[ii - 1].get_i());
585  double distancej =
586  l_controlPoints[i].get_j() - (alfi * tempP[ii + t + 1].get_j() + (1.0 - alfi) * tempP[ii - 1].get_j());
587  double distancew = l_weights[i] - (alfi * tempW[ii + t + 1] + (1.0 - alfi) * tempW[ii - 1]);
588  double distance = sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew));
589  if (distance <= l_TOL)
590  remflag = 1;
591  }
592  if (remflag == 0)
593  break;
594  else {
595  i = first;
596  j = last;
597  while (j - i > t) {
598  l_controlPoints[i].set_i(tempP[i - off].get_i());
599  l_controlPoints[i].set_j(tempP[i - off].get_j());
600  l_weights[i] = tempW[i - off];
601  l_controlPoints[j].set_i(tempP[j - off].get_i());
602  l_controlPoints[j].set_j(tempP[j - off].get_j());
603  l_weights[j] = tempW[j - off];
604  i++;
605  j--;
606  }
607  }
608  first--;
609  last++;
610  }
611  if (t == 0) {
612  delete[] tempP;
613  delete[] tempW;
614  return t;
615  }
616  for (unsigned int k = l_r + 1; k <= m; k++)
617  l_knots[k - t] = l_knots[k];
618  j = (unsigned int)fout;
619  i = j;
620  for (unsigned int k = 1; k < t; k++) {
621  if (k % 2)
622  i++;
623  else
624  j--;
625  }
626  for (unsigned int k = i + 1; k <= n; k++) {
627  l_controlPoints[j].set_i(l_controlPoints[k].get_i());
628  l_controlPoints[j].set_j(l_controlPoints[k].get_j());
629  l_weights[j] = l_weights[k];
630  j++;
631  }
632  for (unsigned int k = 0; k < t; k++) {
633  l_knots.erase(l_knots.end() - 1);
634  l_controlPoints.erase(l_controlPoints.end() - 1);
635  }
636 
637  for (unsigned int k = 0; k < l_controlPoints.size(); k++)
638  l_controlPoints[k].set_ij(l_controlPoints[k].get_i() / l_weights[k], l_controlPoints[k].get_j() / l_weights[k]);
639 
640  delete[] tempP;
641  delete[] tempW;
642  return t;
643 }
644 
665 unsigned int vpNurbs::removeCurveKnot(double u, unsigned int r, unsigned int num, double TOL)
666 {
667  return removeCurveKnot(u, r, num, TOL, 0, p, knots, controlPoints, weights);
668 }
669 
682 void vpNurbs::globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p,
683  std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
684  std::vector<double> &l_weights)
685 {
686  if (l_p == 0) {
687  // vpERROR_TRACE("Bad degree of the NURBS basis functions");
688  throw(vpException(vpException::badValue, "Bad degree of the NURBS basis functions"));
689  }
690 
691  l_knots.clear();
692  l_controlPoints.clear();
693  l_weights.clear();
694  unsigned int n = (unsigned int)l_crossingPoints.size() - 1;
695  unsigned int m = n + l_p + 1;
696 
697  double d = 0;
698  for (unsigned int k = 1; k <= n; k++)
699  d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
700 
701  // Compute ubar
702  std::vector<double> ubar;
703  ubar.push_back(0.0);
704  for (unsigned int k = 1; k < n; k++) {
705  ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
706  }
707  ubar.push_back(1.0);
708 
709  // Compute the knot vector
710  for (unsigned int k = 0; k <= l_p; k++)
711  l_knots.push_back(0.0);
712 
713  double sum = 0;
714  for (unsigned int k = 1; k <= l_p; k++)
715  sum = sum + ubar[k];
716 
717  // Centripetal method
718  for (unsigned int k = 1; k <= n - l_p; k++) {
719  l_knots.push_back(sum / l_p);
720  sum = sum - ubar[k - 1] + ubar[l_p + k - 1];
721  }
722 
723  for (unsigned int k = m - l_p; k <= m; k++)
724  l_knots.push_back(1.0);
725 
726  vpMatrix A(n + 1, n + 1);
727  vpBasisFunction *N;
728 
729  for (unsigned int i = 0; i <= n; i++) {
730  unsigned int span = findSpan(ubar[i], l_p, l_knots);
731  N = computeBasisFuns(ubar[i], span, l_p, l_knots);
732  for (unsigned int k = 0; k <= l_p; k++)
733  A[i][span - l_p + k] = N[k].value;
734  delete[] N;
735  }
736  // vpMatrix Ainv = A.inverseByLU();
737  vpMatrix Ainv;
738  A.pseudoInverse(Ainv);
739  vpColVector Qi(n + 1);
740  vpColVector Qj(n + 1);
741  vpColVector Qw(n + 1);
742  for (unsigned int k = 0; k <= n; k++) {
743  Qi[k] = l_crossingPoints[k].get_i();
744  Qj[k] = l_crossingPoints[k].get_j();
745  }
746  Qw = 1;
747  vpColVector Pi = Ainv * Qi;
748  vpColVector Pj = Ainv * Qj;
749  vpColVector Pw = Ainv * Qw;
750 
751  vpImagePoint pt;
752  for (unsigned int k = 0; k <= n; k++) {
753  pt.set_ij(Pi[k], Pj[k]);
754  l_controlPoints.push_back(pt);
755  l_weights.push_back(Pw[k]);
756  }
757 }
758 
770 {
771  std::vector<vpImagePoint> v_crossingPoints;
772  l_crossingPoints.front();
773  vpMeSite s = l_crossingPoints.value();
774  vpImagePoint pt(s.ifloat, s.jfloat);
775  vpImagePoint pt_1 = pt;
776  v_crossingPoints.push_back(pt);
777  l_crossingPoints.next();
778  while (!l_crossingPoints.outside()) {
779  s = l_crossingPoints.value();
780  pt.set_ij(s.ifloat, s.jfloat);
781  if (vpImagePoint::distance(pt_1, pt) >= 10) {
782  v_crossingPoints.push_back(pt);
783  pt_1 = pt;
784  }
785  l_crossingPoints.next();
786  }
787  globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights);
788 }
789 
800 void vpNurbs::globalCurveInterp(const std::list<vpImagePoint> &l_crossingPoints)
801 {
802  std::vector<vpImagePoint> v_crossingPoints;
803  for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
804  v_crossingPoints.push_back(*it);
805  }
806  globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights);
807 }
808 
819 void vpNurbs::globalCurveInterp(const std::list<vpMeSite> &l_crossingPoints)
820 {
821  std::vector<vpImagePoint> v_crossingPoints;
822  vpMeSite s = l_crossingPoints.front();
823  vpImagePoint pt(s.ifloat, s.jfloat);
824  vpImagePoint pt_1 = pt;
825  v_crossingPoints.push_back(pt);
826  std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
827  ++it;
828  for (; it != l_crossingPoints.end(); ++it) {
829  vpImagePoint pt_tmp(it->ifloat, it->jfloat);
830  if (vpImagePoint::distance(pt_1, pt_tmp) >= 10) {
831  v_crossingPoints.push_back(pt_tmp);
832  pt_1 = pt_tmp;
833  }
834  }
835  globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights);
836 }
837 
845 void vpNurbs::globalCurveInterp() { globalCurveInterp(crossingPoints, p, knots, controlPoints, weights); }
846 
863 void vpNurbs::globalCurveApprox(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n,
864  std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
865  std::vector<double> &l_weights)
866 {
867  l_knots.clear();
868  l_controlPoints.clear();
869  l_weights.clear();
870  unsigned int m = (unsigned int)l_crossingPoints.size() - 1;
871 
872  double d = 0;
873  for (unsigned int k = 1; k <= m; k++)
874  d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
875 
876  // Compute ubar
877  std::vector<double> ubar;
878  ubar.push_back(0.0);
879  for (unsigned int k = 1; k < m; k++)
880  ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
881  ubar.push_back(1.0);
882 
883  // Compute the knot vector
884  for (unsigned int k = 0; k <= l_p; k++)
885  l_knots.push_back(0.0);
886 
887  d = (double)(m + 1) / (double)(l_n - l_p + 1);
888 
889  for (unsigned int j = 1; j <= l_n - l_p; j++) {
890  double i = floor(j * d);
891  double alpha = j * d - i;
892  l_knots.push_back((1.0 - alpha) * ubar[(unsigned int)i - 1] + alpha * ubar[(unsigned int)i]);
893  }
894 
895  for (unsigned int k = 0; k <= l_p; k++)
896  l_knots.push_back(1.0);
897 
898  // Compute Rk
899  std::vector<vpImagePoint> Rk;
900  vpBasisFunction *N;
901  for (unsigned int k = 1; k <= m - 1; k++) {
902  unsigned int span = findSpan(ubar[k], l_p, l_knots);
903  if (span == l_p && span == l_n) {
904  N = computeBasisFuns(ubar[k], span, l_p, l_knots);
905  vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i() -
906  N[l_p].value * l_crossingPoints[m].get_i(),
907  l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j() -
908  N[l_p].value * l_crossingPoints[m].get_j());
909  Rk.push_back(pt);
910  delete[] N;
911  } else if (span == l_p) {
912  N = computeBasisFuns(ubar[k], span, l_p, l_knots);
913  vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i(),
914  l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j());
915  Rk.push_back(pt);
916  delete[] N;
917  } else if (span == l_n) {
918  N = computeBasisFuns(ubar[k], span, l_p, l_knots);
919  vpImagePoint pt(l_crossingPoints[k].get_i() - N[l_p].value * l_crossingPoints[m].get_i(),
920  l_crossingPoints[k].get_j() - N[l_p].value * l_crossingPoints[m].get_j());
921  Rk.push_back(pt);
922  delete[] N;
923  } else {
924  Rk.push_back(l_crossingPoints[k]);
925  }
926  }
927 
928  vpMatrix A(m - 1, l_n - 1);
929  // Compute A
930  for (unsigned int i = 1; i <= m - 1; i++) {
931  unsigned int span = findSpan(ubar[i], l_p, l_knots);
932  N = computeBasisFuns(ubar[i], span, l_p, l_knots);
933  for (unsigned int k = 0; k <= l_p; k++) {
934  if (N[k].i > 0 && N[k].i < l_n)
935  A[i - 1][N[k].i - 1] = N[k].value;
936  }
937  delete[] N;
938  }
939 
940  vpColVector Ri(l_n - 1);
941  vpColVector Rj(l_n - 1);
942  vpColVector Rw(l_n - 1);
943  for (unsigned int i = 0; i < l_n - 1; i++) {
944  double sum = 0;
945  for (unsigned int k = 0; k < m - 1; k++)
946  sum = sum + A[k][i] * Rk[k].get_i();
947  Ri[i] = sum;
948  sum = 0;
949  for (unsigned int k = 0; k < m - 1; k++)
950  sum = sum + A[k][i] * Rk[k].get_j();
951  Rj[i] = sum;
952  sum = 0;
953  for (unsigned int k = 0; k < m - 1; k++)
954  sum = sum + A[k][i]; // The crossing points weigths are equal to 1.
955  Rw[i] = sum;
956  }
957 
958  vpMatrix AtA = A.AtA();
959  vpMatrix AtAinv;
960  AtA.pseudoInverse(AtAinv);
961 
962  vpColVector Pi = AtAinv * Ri;
963  vpColVector Pj = AtAinv * Rj;
964  vpColVector Pw = AtAinv * Rw;
965 
966  vpImagePoint pt;
967  l_controlPoints.push_back(l_crossingPoints[0]);
968  l_weights.push_back(1.0);
969  for (unsigned int k = 0; k < l_n - 1; k++) {
970  pt.set_ij(Pi[k], Pj[k]);
971  l_controlPoints.push_back(pt);
972  l_weights.push_back(Pw[k]);
973  }
974  l_controlPoints.push_back(l_crossingPoints[m]);
975  l_weights.push_back(1.0);
976 }
977 
994 void vpNurbs::globalCurveApprox(vpList<vpMeSite> &l_crossingPoints, unsigned int n)
995 {
996  std::vector<vpImagePoint> v_crossingPoints;
997  l_crossingPoints.front();
998  while (!l_crossingPoints.outside()) {
999  vpMeSite s = l_crossingPoints.value();
1000  vpImagePoint pt(s.ifloat, s.jfloat);
1001  v_crossingPoints.push_back(pt);
1002  l_crossingPoints.next();
1003  }
1004  globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights);
1005 }
1006 
1020 void vpNurbs::globalCurveApprox(const std::list<vpImagePoint> &l_crossingPoints, unsigned int n)
1021 {
1022  std::vector<vpImagePoint> v_crossingPoints;
1023  for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
1024  v_crossingPoints.push_back(*it);
1025  }
1026  globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights);
1027 }
1028 
1045 void vpNurbs::globalCurveApprox(const std::list<vpMeSite> &l_crossingPoints, unsigned int n)
1046 {
1047  std::vector<vpImagePoint> v_crossingPoints;
1048  for (std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
1049  vpImagePoint pt(it->ifloat, it->jfloat);
1050  v_crossingPoints.push_back(pt);
1051  }
1052  globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights);
1053 }
1054 
1064 void vpNurbs::globalCurveApprox(unsigned int n)
1065 {
1066  globalCurveApprox(crossingPoints, p, n, knots, controlPoints, weights);
1067 }
Class that provides tools to compute and manipulate a B-Spline curve.
Definition: vpBSpline.h:111
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 vpBasisFunction * computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots)
Definition: vpBSpline.cpp:147
static unsigned int findSpan(double l_u, unsigned int l_p, std::vector< double > &l_knots)
Definition: vpBSpline.cpp:84
Implementation of column vector and the associated operations.
Definition: vpColVector.h:172
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
void set_j(double jj)
Definition: vpImagePoint.h:309
double get_j() const
Definition: vpImagePoint.h:132
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:320
void set_i(double ii)
Definition: vpImagePoint.h:298
double get_i() const
Definition: vpImagePoint.h:121
Provide simple list management.
Definition: vpList.h:114
void next(void)
position the current element on the next one
Definition: vpList.h:250
void front(void)
Position the current element on the first element of the list.
Definition: vpList.h:323
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Definition: vpList.h:356
type & value(void)
return the value of the current element
Definition: vpList.h:269
static double sqr(double x)
Definition: vpMath.h:122
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:307
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix AtA() const
Definition: vpMatrix.cpp:623
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2232
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5970
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:67
double ifloat
Definition: vpMeSite.h:88
double jfloat
Definition: vpMeSite.h:88
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
Definition: vpNurbs.h:98
static void refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:404
std::vector< double > weights
Definition: vpNurbs.h:100
static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:82
static void curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:322
vpNurbs()
Definition: vpNurbs.cpp:57
static vpMatrix 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 > &l_weights)
Definition: vpNurbs.cpp:167
static vpImagePoint * computeCurveDersPoint(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 > &l_weights)
Definition: vpNurbs.cpp:257
static void globalCurveApprox(std::vector< vpImagePoint > &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:863
virtual ~vpNurbs()
Definition: vpNurbs.cpp:67
static unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s, unsigned int l_p, std::vector< double > &l_knots, std::vector< vpImagePoint > &l_controlPoints, std::vector< double > &l_weights)
Definition: vpNurbs.cpp:523
void globalCurveInterp()
Definition: vpNurbs.cpp:845