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