Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpNurbs.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * This class implements the Non Uniform Rational B-Spline (NURBS)
32 */
33
34#include <cmath> // std::fabs
35#include <limits> // numeric_limits
36#include <visp3/core/vpColVector.h>
37#include <visp3/me/vpNurbs.h>
38/*
39 Compute the distance d = |Pw1-Pw2|
40*/
41inline double distance(const vpImagePoint &iP1, double w1, const vpImagePoint &iP2, double w2)
42{
43 double distancei = iP1.get_i() - iP2.get_i();
44 double distancej = iP1.get_j() - iP2.get_j();
45 double distancew = w1 - w2;
46 return sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew));
47}
48
49vpNurbs::vpNurbs() : weights() { p = 3; }
50
51vpNurbs::vpNurbs(const vpNurbs &nurbs) : vpBSpline(nurbs), weights(nurbs.weights) { }
52
54
55vpImagePoint vpNurbs::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots,
56 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
57{
58 vpBasisFunction *N = NULL;
59 N = computeBasisFuns(l_u, l_i, l_p, l_knots);
60 vpImagePoint pt;
61
62 double ic = 0;
63 double jc = 0;
64 double wc = 0;
65 for (unsigned int j = 0; j <= l_p; j++) {
66 ic = ic + N[j].value * (l_controlPoints[l_i - l_p + j]).get_i() * l_weights[l_i - l_p + j];
67 jc = jc + N[j].value * (l_controlPoints[l_i - l_p + j]).get_j() * l_weights[l_i - l_p + j];
68 wc = wc + N[j].value * l_weights[l_i - l_p + j];
69 }
70
71 pt.set_i(ic / wc);
72 pt.set_j(jc / wc);
73
74 if (N != NULL)
75 delete[] N;
76
77 return pt;
78}
79
81{
82 vpBasisFunction *N = NULL;
83 N = computeBasisFuns(u);
84 vpImagePoint pt;
85
86 double ic = 0;
87 double jc = 0;
88 double wc = 0;
89 for (unsigned int j = 0; j <= p; j++) {
90 ic = ic + N[j].value * (controlPoints[N[0].i + j]).get_i() * weights[N[0].i + j]; // N[0].i = findSpan(u)-p
91 jc = jc + N[j].value * (controlPoints[N[0].i + j]).get_j() * weights[N[0].i + j];
92 wc = wc + N[j].value * weights[N[0].i + j];
93 }
94
95 pt.set_i(ic / wc);
96 pt.set_j(jc / wc);
97
98 if (N != NULL)
99 delete[] N;
100
101 return pt;
102}
103
104vpMatrix vpNurbs::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
105 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
106 std::vector<double> &l_weights)
107{
108 vpMatrix derivate(l_der + 1, 3);
109 vpBasisFunction **N = NULL;
110 N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
111
112 for (unsigned int k = 0; k <= l_der; k++) {
113 derivate[k][0] = 0.0;
114 derivate[k][1] = 0.0;
115 derivate[k][2] = 0.0;
116
117 for (unsigned int j = 0; j <= l_p; j++) {
118 derivate[k][0] = derivate[k][0] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_i();
119 derivate[k][1] = derivate[k][1] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_j();
120 derivate[k][2] = derivate[k][2] + N[k][j].value * (l_weights[l_i - l_p + j]);
121 }
122 }
123
124 if (N != NULL) {
125 for (unsigned int i = 0; i <= l_der; i++)
126 delete[] N[i];
127 delete[] N;
128 }
129 return derivate;
130}
131
132vpMatrix vpNurbs::computeCurveDers(double u, unsigned int der)
133{
134 vpMatrix derivate(der + 1, 3);
135 vpBasisFunction **N = NULL;
136 N = computeDersBasisFuns(u, der);
137
138 for (unsigned int k = 0; k <= der; k++) {
139 derivate[k][0] = 0.0;
140 derivate[k][1] = 0.0;
141 derivate[k][2] = 0.0;
142 for (unsigned int j = 0; j <= p; j++) {
143 derivate[k][0] = derivate[k][0] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_i();
144 derivate[k][1] = derivate[k][1] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_j();
145 derivate[k][2] = derivate[k][2] + N[k][j].value * (weights[N[0][0].i - p + j]);
146 }
147 }
148
149 if (N != NULL)
150 delete[] N;
151
152 return derivate;
153}
154
155vpImagePoint *vpNurbs::computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
156 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
157 std::vector<double> &l_weights)
158{
159 std::vector<vpImagePoint> A;
160 vpImagePoint pt;
161 for (unsigned int j = 0; j < l_controlPoints.size(); j++) {
162 pt = l_controlPoints[j];
163 pt.set_i(pt.get_i() * l_weights[j]);
164 pt.set_j(pt.get_j() * l_weights[j]);
165 A.push_back(pt);
166 }
167
168 vpMatrix Awders = computeCurveDers(l_u, l_i, l_p, l_der, l_knots, A, l_weights);
169
170 vpImagePoint *CK = new vpImagePoint[l_der + 1];
171
172 for (unsigned int k = 0; k <= l_der; k++) {
173 double ic = Awders[k][0];
174 double jc = Awders[k][1];
175 for (unsigned int j = 1; j <= k; j++) {
176 double tmpComb = static_cast<double>(vpMath::comb(k, j));
177 ic = ic - tmpComb * Awders[k][2] * (CK[k - j].get_i());
178 jc = jc - tmpComb * Awders[j][2] * (CK[k - j].get_j());
179 }
180 CK[k].set_ij(ic / Awders[0][2], jc / Awders[0][2]);
181 }
182 return CK;
183}
184
185
187{
188 unsigned int i = findSpan(u);
189 return computeCurveDersPoint(u, i, p, der, knots, controlPoints, weights);
190}
191
192
193void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p,
194 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
195 std::vector<double> &l_weights)
196{
197 vpMatrix Rw(l_p + 1, 3);
198 std::vector<vpImagePoint>::iterator it1;
199 std::vector<double>::iterator it2;
200 vpImagePoint pt;
201 double w = 0;
202
203 for (unsigned int j = 0; j <= l_p - l_s; j++) {
204 Rw[j][0] = (l_controlPoints[l_k - l_p + j]).get_i() * l_weights[l_k - l_p + j];
205 Rw[j][1] = (l_controlPoints[l_k - l_p + j]).get_j() * l_weights[l_k - l_p + j];
206 Rw[j][2] = l_weights[l_k - l_p + j];
207 }
208
209 it1 = l_controlPoints.begin();
210 l_controlPoints.insert(it1 + (int)l_k - (int)l_s, l_r, pt);
211 it2 = l_weights.begin();
212 l_weights.insert(it2 + (int)l_k - (int)l_s, l_r, w);
213
214 unsigned int L = 0;
215 double alpha;
216 for (unsigned int j = 1; j <= l_r; j++) {
217 L = l_k - l_p + j;
218
219 for (unsigned int i = 0; i <= l_p - j - l_s; i++) {
220 alpha = (l_u - l_knots[L + i]) / (l_knots[i + l_k + 1] - l_knots[L + i]);
221 Rw[i][0] = alpha * Rw[i + 1][0] + (1.0 - alpha) * Rw[i][0];
222 Rw[i][1] = alpha * Rw[i + 1][1] + (1.0 - alpha) * Rw[i][1];
223 Rw[i][2] = alpha * Rw[i + 1][2] + (1.0 - alpha) * Rw[i][2];
224 }
225
226 pt.set_ij(Rw[0][0] / Rw[0][2], Rw[0][1] / Rw[0][2]);
227 l_controlPoints[L] = pt;
228 l_weights[L] = Rw[0][2];
229
230 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]);
231 l_controlPoints[l_k + l_r - j - l_s] = pt;
232 l_weights[l_k + l_r - j - l_s] = Rw[l_p - j - l_s][2];
233 }
234
235 for (unsigned int j = L + 1; j < l_k - l_s; j++) {
236 pt.set_ij(Rw[j - L][0] / Rw[j - L][2], Rw[j - L][1] / Rw[j - L][2]);
237 l_controlPoints[j] = pt;
238 l_weights[j] = Rw[j - L][2];
239 }
240
241 it2 = l_knots.begin();
242 l_knots.insert(it2 + (int)l_k, l_r, l_u);
243}
244
245void vpNurbs::curveKnotIns(double u, unsigned int s, unsigned int r)
246{
247 unsigned int i = findSpan(u);
248 curveKnotIns(u, i, s, r, p, knots, controlPoints, weights);
249}
250
251
252void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots,
253 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
254{
255 unsigned int a = findSpan(l_x[0], l_p, l_knots);
256 unsigned int b = findSpan(l_x[l_r], l_p, l_knots);
257 b++;
258
259 unsigned int n = (unsigned int)l_controlPoints.size();
260 unsigned int m = (unsigned int)l_knots.size();
261
262 for (unsigned int j = 0; j < n; j++) {
263 l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
264 }
265
266 std::vector<double> l_knots_tmp(l_knots);
267 std::vector<vpImagePoint> l_controlPoints_tmp(l_controlPoints);
268 std::vector<double> l_weights_tmp(l_weights);
269
270 vpImagePoint pt;
271 double w = 0;
272
273 for (unsigned int j = 0; j <= l_r; j++) {
274 l_controlPoints.push_back(pt);
275 l_weights.push_back(w);
276 l_knots.push_back(w);
277 }
278
279 for (unsigned int j = b + l_p; j <= m - 1; j++)
280 l_knots[j + l_r + 1] = l_knots_tmp[j];
281
282 for (unsigned int j = b - 1; j <= n - 1; j++) {
283 l_controlPoints[j + l_r + 1] = l_controlPoints_tmp[j];
284 l_weights[j + l_r + 1] = l_weights_tmp[j];
285 }
286
287 unsigned int i = b + l_p - 1;
288 unsigned int k = b + l_p + l_r;
289
290 {
291 unsigned int j = l_r + 1;
292 do {
293 j--;
294 while (l_x[j] <= l_knots[i] && i > a) {
295 l_controlPoints[k - l_p - 1] = l_controlPoints_tmp[i - l_p - 1];
296 l_weights[k - l_p - 1] = l_weights_tmp[i - l_p - 1];
297 l_knots[k] = l_knots_tmp[i];
298 k--;
299 i--;
300 }
301
302 l_controlPoints[k - l_p - 1] = l_controlPoints[k - l_p];
303 l_weights[k - l_p - 1] = l_weights[k - l_p];
304
305 for (unsigned int l = 1; l <= l_p; l++) {
306 unsigned int ind = k - l_p + l;
307 double alpha = l_knots[k + l] - l_x[j];
308 // if (vpMath::abs(alpha) == 0.0)
309 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon()) {
310 l_controlPoints[ind - 1] = l_controlPoints[ind];
311 l_weights[ind - 1] = l_weights[ind];
312 }
313 else {
314 alpha = alpha / (l_knots[k + l] - l_knots_tmp[i - l_p + l]);
315 l_controlPoints[ind - 1].set_i(alpha * l_controlPoints[ind - 1].get_i() +
316 (1.0 - alpha) * l_controlPoints[ind].get_i());
317 l_controlPoints[ind - 1].set_j(alpha * l_controlPoints[ind - 1].get_j() +
318 (1.0 - alpha) * l_controlPoints[ind].get_j());
319 l_weights[ind - 1] = alpha * l_weights[ind - 1] + (1.0 - alpha) * l_weights[ind];
320 }
321 }
322 l_knots[k] = l_x[j];
323 k--;
324 } while (j != 0);
325 }
326
327 for (unsigned int j = 0; j < n; j++) {
328 l_controlPoints[j].set_ij(l_controlPoints[j].get_i() / l_weights[j], l_controlPoints[j].get_j() / l_weights[j]);
329 }
330}
331
332void vpNurbs::refineKnotVectCurve(double *x, unsigned int r)
333{
334 refineKnotVectCurve(x, r, p, knots, controlPoints, weights);
335}
336
337unsigned int vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s,
338 unsigned int l_p, std::vector<double> &l_knots,
339 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
340{
341 unsigned int n = (unsigned int)l_controlPoints.size();
342 unsigned int m = n + l_p + 1;
343
344 for (unsigned int j = 0; j < n; j++) {
345 l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
346 }
347
348 unsigned int ord = l_p + 1;
349 double fout = (2 * l_r - l_s - l_p) / 2.;
350 unsigned int last = l_r - l_s;
351 unsigned int first = l_r - l_p;
352 unsigned int tblSize = 2 * l_p + 1;
353 vpImagePoint *tempP = new vpImagePoint[tblSize];
354 double *tempW = new double[tblSize];
355 vpImagePoint pt;
356 unsigned int t = 0;
357 double alfi = 0;
358 double alfj = 0;
359 unsigned int i, j;
360
361 for (t = 0; t < l_num; t++) {
362 unsigned int off = first - 1;
363 tempP[0] = l_controlPoints[off];
364 tempW[0] = l_weights[off];
365 tempP[last + 1 - off] = l_controlPoints[last + 1];
366 tempW[last + 1 - off] = l_weights[last + 1];
367 i = first;
368 j = last;
369 unsigned int ii = 1;
370 unsigned int jj = last - off;
371 int remflag = 0;
372 while (j - i > t) {
373 alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
374 alfj = (l_u - l_knots[j - t]) / (l_knots[j + ord] - l_knots[j - t]);
375 pt.set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
376 tempP[ii].set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
377 tempP[ii].set_j((l_controlPoints[i].get_j() - (1.0 - alfi) * tempP[ii - 1].get_j()) / alfi);
378 tempW[ii] = ((l_weights[i] - (1.0 - alfi) * tempW[ii - 1]) / alfi);
379 tempP[jj].set_i((l_controlPoints[j].get_i() - alfj * tempP[jj + 1].get_i()) / (1.0 - alfj));
380 tempP[jj].set_j((l_controlPoints[j].get_j() - alfj * tempP[jj + 1].get_j()) / (1.0 - alfj));
381 tempW[jj] = ((l_weights[j] - alfj * tempW[jj + 1]) / (1.0 - alfj));
382 i++;
383 j--;
384 ii++;
385 jj--;
386 }
387
388 if (j - i < t) {
389 double distancei = tempP[ii - 1].get_i() - tempP[jj + 1].get_i();
390 double distancej = tempP[ii - 1].get_j() - tempP[jj + 1].get_j();
391 double distancew = tempW[ii - 1] - tempW[jj + 1];
392 double distance = sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew));
393 if (distance <= l_TOL)
394 remflag = 1;
395 }
396 else {
397 alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
398 double distancei =
399 l_controlPoints[i].get_i() - (alfi * tempP[ii + t + 1].get_i() + (1.0 - alfi) * tempP[ii - 1].get_i());
400 double distancej =
401 l_controlPoints[i].get_j() - (alfi * tempP[ii + t + 1].get_j() + (1.0 - alfi) * tempP[ii - 1].get_j());
402 double distancew = l_weights[i] - (alfi * tempW[ii + t + 1] + (1.0 - alfi) * tempW[ii - 1]);
403 double distance = sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew));
404 if (distance <= l_TOL)
405 remflag = 1;
406 }
407 if (remflag == 0)
408 break;
409 else {
410 i = first;
411 j = last;
412 while (j - i > t) {
413 l_controlPoints[i].set_i(tempP[i - off].get_i());
414 l_controlPoints[i].set_j(tempP[i - off].get_j());
415 l_weights[i] = tempW[i - off];
416 l_controlPoints[j].set_i(tempP[j - off].get_i());
417 l_controlPoints[j].set_j(tempP[j - off].get_j());
418 l_weights[j] = tempW[j - off];
419 i++;
420 j--;
421 }
422 }
423 first--;
424 last++;
425 }
426 if (t == 0) {
427 delete[] tempP;
428 delete[] tempW;
429 return t;
430 }
431 for (unsigned int k = l_r + 1; k <= m; k++)
432 l_knots[k - t] = l_knots[k];
433 j = (unsigned int)fout;
434 i = j;
435 for (unsigned int k = 1; k < t; k++) {
436 if (k % 2)
437 i++;
438 else
439 j--;
440 }
441 for (unsigned int k = i + 1; k <= n; k++) {
442 l_controlPoints[j].set_i(l_controlPoints[k].get_i());
443 l_controlPoints[j].set_j(l_controlPoints[k].get_j());
444 l_weights[j] = l_weights[k];
445 j++;
446 }
447 for (unsigned int k = 0; k < t; k++) {
448 l_knots.erase(l_knots.end() - 1);
449 l_controlPoints.erase(l_controlPoints.end() - 1);
450 }
451
452 for (unsigned int k = 0; k < l_controlPoints.size(); k++)
453 l_controlPoints[k].set_ij(l_controlPoints[k].get_i() / l_weights[k], l_controlPoints[k].get_j() / l_weights[k]);
454
455 delete[] tempP;
456 delete[] tempW;
457 return t;
458}
459
460unsigned int vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL)
461{
462 return removeCurveKnot(l_u, l_r, l_num, l_TOL, 0, p, knots, controlPoints, weights);
463}
464
465void vpNurbs::globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p,
466 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
467 std::vector<double> &l_weights)
468{
469 if (l_p == 0) {
470 // vpERROR_TRACE("Bad degree of the NURBS basis functions");
471 throw(vpException(vpException::badValue, "Bad degree of the NURBS basis functions"));
472 }
473
474 l_knots.clear();
475 l_controlPoints.clear();
476 l_weights.clear();
477 unsigned int n = (unsigned int)l_crossingPoints.size() - 1;
478 unsigned int m = n + l_p + 1;
479
480 double d = 0;
481 for (unsigned int k = 1; k <= n; k++)
482 d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
483
484 // Compute ubar
485 std::vector<double> ubar;
486 ubar.push_back(0.0);
487 for (unsigned int k = 1; k < n; k++) {
488 ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
489 }
490 ubar.push_back(1.0);
491
492 // Compute the knot vector
493 for (unsigned int k = 0; k <= l_p; k++)
494 l_knots.push_back(0.0);
495
496 double sum = 0;
497 for (unsigned int k = 1; k <= l_p; k++)
498 sum = sum + ubar[k];
499
500 // Centripetal method
501 for (unsigned int k = 1; k <= n - l_p; k++) {
502 l_knots.push_back(sum / l_p);
503 sum = sum - ubar[k - 1] + ubar[l_p + k - 1];
504 }
505
506 for (unsigned int k = m - l_p; k <= m; k++)
507 l_knots.push_back(1.0);
508
509 vpMatrix A(n + 1, n + 1);
510 vpBasisFunction *N;
511
512 for (unsigned int i = 0; i <= n; i++) {
513 unsigned int span = findSpan(ubar[i], l_p, l_knots);
514 N = computeBasisFuns(ubar[i], span, l_p, l_knots);
515 for (unsigned int k = 0; k <= l_p; k++)
516 A[i][span - l_p + k] = N[k].value;
517 delete[] N;
518 }
519 // vpMatrix Ainv = A.inverseByLU();
520 vpMatrix Ainv;
521 A.pseudoInverse(Ainv);
522 vpColVector Qi(n + 1);
523 vpColVector Qj(n + 1);
524 vpColVector Qw(n + 1);
525 for (unsigned int k = 0; k <= n; k++) {
526 Qi[k] = l_crossingPoints[k].get_i();
527 Qj[k] = l_crossingPoints[k].get_j();
528 }
529 Qw = 1;
530 vpColVector Pi = Ainv * Qi;
531 vpColVector Pj = Ainv * Qj;
532 vpColVector Pw = Ainv * Qw;
533
534 vpImagePoint pt;
535 for (unsigned int k = 0; k <= n; k++) {
536 pt.set_ij(Pi[k], Pj[k]);
537 l_controlPoints.push_back(pt);
538 l_weights.push_back(Pw[k]);
539 }
540}
541
543{
544 std::vector<vpImagePoint> v_crossingPoints;
545 l_crossingPoints.front();
546 vpMeSite s = l_crossingPoints.value();
547 vpImagePoint pt(s.ifloat, s.jfloat);
548 vpImagePoint pt_1 = pt;
549 v_crossingPoints.push_back(pt);
550 l_crossingPoints.next();
551 while (!l_crossingPoints.outside()) {
552 s = l_crossingPoints.value();
553 pt.set_ij(s.ifloat, s.jfloat);
554 if (vpImagePoint::distance(pt_1, pt) >= 10) {
555 v_crossingPoints.push_back(pt);
556 pt_1 = pt;
557 }
558 l_crossingPoints.next();
559 }
560 globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights);
561}
562
563void vpNurbs::globalCurveInterp(const std::list<vpImagePoint> &l_crossingPoints)
564{
565 std::vector<vpImagePoint> v_crossingPoints;
566 for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
567 v_crossingPoints.push_back(*it);
568 }
569 globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights);
570}
571
572
573void vpNurbs::globalCurveInterp(const std::list<vpMeSite> &l_crossingPoints)
574{
575 std::vector<vpImagePoint> v_crossingPoints;
576 vpMeSite s = l_crossingPoints.front();
577 vpImagePoint pt(s.ifloat, s.jfloat);
578 vpImagePoint pt_1 = pt;
579 v_crossingPoints.push_back(pt);
580 std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
581 ++it;
582 for (; it != l_crossingPoints.end(); ++it) {
583 vpImagePoint pt_tmp(it->ifloat, it->jfloat);
584 if (vpImagePoint::distance(pt_1, pt_tmp) >= 10) {
585 v_crossingPoints.push_back(pt_tmp);
586 pt_1 = pt_tmp;
587 }
588 }
589 globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights);
590}
591
593
594void vpNurbs::globalCurveApprox(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n,
595 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
596 std::vector<double> &l_weights)
597{
598 l_knots.clear();
599 l_controlPoints.clear();
600 l_weights.clear();
601 unsigned int m = (unsigned int)l_crossingPoints.size() - 1;
602
603 double d = 0;
604 for (unsigned int k = 1; k <= m; k++)
605 d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
606
607 // Compute ubar
608 std::vector<double> ubar;
609 ubar.push_back(0.0);
610 for (unsigned int k = 1; k < m; k++)
611 ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
612 ubar.push_back(1.0);
613
614 // Compute the knot vector
615 for (unsigned int k = 0; k <= l_p; k++)
616 l_knots.push_back(0.0);
617
618 d = (double)(m + 1) / (double)(l_n - l_p + 1);
619
620 for (unsigned int j = 1; j <= l_n - l_p; j++) {
621 double i = floor(j * d);
622 double alpha = j * d - i;
623 l_knots.push_back((1.0 - alpha) * ubar[(unsigned int)i - 1] + alpha * ubar[(unsigned int)i]);
624 }
625
626 for (unsigned int k = 0; k <= l_p; k++)
627 l_knots.push_back(1.0);
628
629 // Compute Rk
630 std::vector<vpImagePoint> Rk;
631 vpBasisFunction *N;
632 for (unsigned int k = 1; k <= m - 1; k++) {
633 unsigned int span = findSpan(ubar[k], l_p, l_knots);
634 if (span == l_p && span == l_n) {
635 N = computeBasisFuns(ubar[k], span, l_p, l_knots);
636 vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i() -
637 N[l_p].value * l_crossingPoints[m].get_i(),
638 l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j() -
639 N[l_p].value * l_crossingPoints[m].get_j());
640 Rk.push_back(pt);
641 delete[] N;
642 }
643 else if (span == l_p) {
644 N = computeBasisFuns(ubar[k], span, l_p, l_knots);
645 vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i(),
646 l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j());
647 Rk.push_back(pt);
648 delete[] N;
649 }
650 else if (span == l_n) {
651 N = computeBasisFuns(ubar[k], span, l_p, l_knots);
652 vpImagePoint pt(l_crossingPoints[k].get_i() - N[l_p].value * l_crossingPoints[m].get_i(),
653 l_crossingPoints[k].get_j() - N[l_p].value * l_crossingPoints[m].get_j());
654 Rk.push_back(pt);
655 delete[] N;
656 }
657 else {
658 Rk.push_back(l_crossingPoints[k]);
659 }
660 }
661
662 vpMatrix A(m - 1, l_n - 1);
663 // Compute A
664 for (unsigned int i = 1; i <= m - 1; i++) {
665 unsigned int span = findSpan(ubar[i], l_p, l_knots);
666 N = computeBasisFuns(ubar[i], span, l_p, l_knots);
667 for (unsigned int k = 0; k <= l_p; k++) {
668 if (N[k].i > 0 && N[k].i < l_n)
669 A[i - 1][N[k].i - 1] = N[k].value;
670 }
671 delete[] N;
672 }
673
674 vpColVector Ri(l_n - 1);
675 vpColVector Rj(l_n - 1);
676 vpColVector Rw(l_n - 1);
677 for (unsigned int i = 0; i < l_n - 1; i++) {
678 double sum = 0;
679 for (unsigned int k = 0; k < m - 1; k++)
680 sum = sum + A[k][i] * Rk[k].get_i();
681 Ri[i] = sum;
682 sum = 0;
683 for (unsigned int k = 0; k < m - 1; k++)
684 sum = sum + A[k][i] * Rk[k].get_j();
685 Rj[i] = sum;
686 sum = 0;
687 for (unsigned int k = 0; k < m - 1; k++)
688 sum = sum + A[k][i]; // The crossing points weigths are equal to 1.
689 Rw[i] = sum;
690 }
691
692 vpMatrix AtA = A.AtA();
693 vpMatrix AtAinv;
694 AtA.pseudoInverse(AtAinv);
695
696 vpColVector Pi = AtAinv * Ri;
697 vpColVector Pj = AtAinv * Rj;
698 vpColVector Pw = AtAinv * Rw;
699
700 vpImagePoint pt;
701 l_controlPoints.push_back(l_crossingPoints[0]);
702 l_weights.push_back(1.0);
703 for (unsigned int k = 0; k < l_n - 1; k++) {
704 pt.set_ij(Pi[k], Pj[k]);
705 l_controlPoints.push_back(pt);
706 l_weights.push_back(Pw[k]);
707 }
708 l_controlPoints.push_back(l_crossingPoints[m]);
709 l_weights.push_back(1.0);
710}
711
712
713void vpNurbs::globalCurveApprox(vpList<vpMeSite> &l_crossingPoints, unsigned int n)
714{
715 std::vector<vpImagePoint> v_crossingPoints;
716 l_crossingPoints.front();
717 while (!l_crossingPoints.outside()) {
718 vpMeSite s = l_crossingPoints.value();
719 vpImagePoint pt(s.ifloat, s.jfloat);
720 v_crossingPoints.push_back(pt);
721 l_crossingPoints.next();
722 }
723 globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights);
724}
725
726
727void vpNurbs::globalCurveApprox(const std::list<vpImagePoint> &l_crossingPoints, unsigned int n)
728{
729 std::vector<vpImagePoint> v_crossingPoints;
730 for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
731 v_crossingPoints.push_back(*it);
732 }
733 globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights);
734}
735
736void vpNurbs::globalCurveApprox(const std::list<vpMeSite> &l_crossingPoints, unsigned int n)
737{
738 std::vector<vpImagePoint> v_crossingPoints;
739 for (std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
740 vpImagePoint pt(it->ifloat, it->jfloat);
741 v_crossingPoints.push_back(pt);
742 }
743 globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights);
744}
745
746void vpNurbs::globalCurveApprox(unsigned int n)
747{
748 globalCurveApprox(crossingPoints, p, n, knots, controlPoints, weights);
749}
Class that provides tools to compute and manipulate a B-Spline curve.
Definition vpBSpline.h:106
static vpBasisFunction ** computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector< double > &l_knots)
static vpBasisFunction * computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots)
unsigned int p
Degree of the B-Spline basis functions.
Definition vpBSpline.h:113
std::vector< vpImagePoint > crossingPoints
Vector wich contains the points used during the interpolation method.
Definition vpBSpline.h:115
std::vector< double > knots
Vector which contain the knots .
Definition vpBSpline.h:111
static unsigned int findSpan(double l_u, unsigned int l_p, std::vector< double > &l_knots)
Definition vpBSpline.cpp:79
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition vpException.h:85
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_j(double jj)
double get_j() const
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(double ii, double jj)
void set_i(double ii)
double get_i() const
Provide simple list management.
Definition vpList.h:108
void next(void)
position the current element on the next one
Definition vpList.h:244
void front(void)
Position the current element on the first element of the list.
Definition vpList.h:317
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Definition vpList.h:350
type & value(void)
return the value of the current element
Definition vpList.h:263
static double sqr(double x)
Definition vpMath.h:124
static long double comb(unsigned int n, unsigned int p)
Definition vpMath.h:309
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpMatrix AtA() const
Definition vpMatrix.cpp:625
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
Definition vpMeSite.h:65
double ifloat
Floating coordinates along i of a site.
Definition vpMeSite.h:100
double jfloat
Floating coordinates along j of a site.
Definition vpMeSite.h:102
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
Definition vpNurbs.h:93
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:252
std::vector< double > weights
Vector which contains the weights associated to each control Points.
Definition vpNurbs.h:96
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:55
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:193
vpNurbs()
Definition vpNurbs.cpp:49
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:104
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:155
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:594
virtual ~vpNurbs()
Definition vpNurbs.cpp:53
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:337
void globalCurveInterp()
Definition vpNurbs.cpp:592