Visual Servoing Platform version 3.5.0
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 * 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*/
46inline double distance(const vpImagePoint &iP1, double w1, const vpImagePoint &iP2, 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
60vpNurbs::vpNurbs() : weights() { p = 3; }
61
65vpNurbs::vpNurbs(const vpNurbs &nurbs) : vpBSpline(nurbs), weights(nurbs.weights) {}
66
71
85vpImagePoint 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
170vpMatrix 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
220vpMatrix 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
260vpImagePoint *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
304{
305 unsigned int i = findSpan(u);
306 return computeCurveDersPoint(u, i, p, der, knots, controlPoints, weights);
307}
308
325void 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
388void 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
407void 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
496void vpNurbs::refineKnotVectCurve(double *x, unsigned int r)
497{
498 refineKnotVectCurve(x, r, p, knots, controlPoints, weights);
499}
500
526unsigned 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
668unsigned 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
685void 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
803void 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
822void 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
848void vpNurbs::globalCurveInterp() { globalCurveInterp(crossingPoints, p, knots, controlPoints, weights); }
849
866void 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
997void 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
1023void 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
1048void 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
1067void vpNurbs::globalCurveApprox(unsigned int n)
1068{
1069 globalCurveApprox(crossingPoints, p, n, knots, controlPoints, weights);
1070}
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:234
static vpBasisFunction * computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots)
Definition: vpBSpline.cpp:148
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:131
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:88
void set_j(double jj)
Definition: vpImagePoint.h:177
double get_j() const
Definition: vpImagePoint.h:214
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(double ii, double jj)
Definition: vpImagePoint.h:188
void set_i(double ii)
Definition: vpImagePoint.h:166
double get_i() const
Definition: vpImagePoint.h:203
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:116
static long double comb(unsigned int n, unsigned int p)
Definition: vpMath.h:232
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix AtA() const
Definition: vpMatrix.cpp:629
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Definition: vpMatrix.cpp:5988
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition: vpMeSite.h:72
double ifloat
Definition: vpMeSite.h:89
double jfloat
Definition: vpMeSite.h:89
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:407
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:85
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
vpNurbs()
Definition: vpNurbs.cpp:60
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
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
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
virtual ~vpNurbs()
Definition: vpNurbs.cpp:70
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
void globalCurveInterp()
Definition: vpNurbs.cpp:848