36#include <visp3/core/vpColVector.h>
37#include <visp3/me/vpNurbs.h>
45 double distancew = w1 - w2;
56 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
58 vpBasisFunction *N = NULL;
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];
82 vpBasisFunction *N = NULL;
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];
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];
105 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
106 std::vector<double> &l_weights)
109 vpBasisFunction **N = NULL;
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;
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]);
125 for (
unsigned int i = 0; i <= l_der; i++)
135 vpBasisFunction **N = NULL;
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]);
156 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
157 std::vector<double> &l_weights)
159 std::vector<vpImagePoint> A;
161 for (
unsigned int j = 0; j < l_controlPoints.size(); j++) {
162 pt = l_controlPoints[j];
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());
180 CK[k].
set_ij(ic / Awders[0][2], jc / Awders[0][2]);
194 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
195 std::vector<double> &l_weights)
198 std::vector<vpImagePoint>::iterator it1;
199 std::vector<double>::iterator it2;
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];
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);
216 for (
unsigned int j = 1; j <= l_r; j++) {
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];
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];
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];
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];
241 it2 = l_knots.begin();
242 l_knots.
insert(it2 + (
int)l_k, l_r, l_u);
253 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
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);
259 unsigned int n = (
unsigned int)l_controlPoints.size();
260 unsigned int m = (
unsigned int)l_knots.size();
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]);
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);
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);
279 for (
unsigned int j = b + l_p; j <= m - 1; j++)
280 l_knots[j + l_r + 1] = l_knots_tmp[j];
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];
287 unsigned int i = b + l_p - 1;
288 unsigned int k = b + l_p + l_r;
291 unsigned int j = l_r + 1;
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];
302 l_controlPoints[k - l_p - 1] = l_controlPoints[k - l_p];
303 l_weights[k - l_p - 1] = l_weights[k - l_p];
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];
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];
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];
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]);
338 unsigned int l_p, std::vector<double> &l_knots,
339 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
341 unsigned int n = (
unsigned int)l_controlPoints.size();
342 unsigned int m = n + l_p + 1;
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]);
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;
354 double *tempW =
new double[tblSize];
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];
370 unsigned int jj = last - off;
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));
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];
393 if (distance <= l_TOL)
397 alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
399 l_controlPoints[i].get_i() - (alfi * tempP[ii + t + 1].
get_i() + (1.0 - alfi) * tempP[ii - 1].get_i());
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]);
404 if (distance <= l_TOL)
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];
431 for (
unsigned int k = l_r + 1; k <= m; k++)
432 l_knots[k - t] = l_knots[k];
433 j = (
unsigned int)fout;
435 for (
unsigned int k = 1; k < t; k++) {
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];
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);
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]);
466 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
467 std::vector<double> &l_weights)
475 l_controlPoints.clear();
477 unsigned int n = (
unsigned int)l_crossingPoints.size() - 1;
478 unsigned int m = n + l_p + 1;
481 for (
unsigned int k = 1; k <= n; k++)
482 d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
485 std::vector<double> ubar;
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);
493 for (
unsigned int k = 0; k <= l_p; k++)
494 l_knots.push_back(0.0);
497 for (
unsigned int k = 1; k <= l_p; k++)
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];
506 for (
unsigned int k = m - l_p; k <= m; k++)
507 l_knots.push_back(1.0);
512 for (
unsigned int i = 0; i <= n; i++) {
513 unsigned int span =
findSpan(ubar[i], l_p, l_knots);
515 for (
unsigned int k = 0; k <= l_p; k++)
516 A[i][span - l_p + k] = N[k].value;
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();
535 for (
unsigned int k = 0; k <= n; k++) {
537 l_controlPoints.push_back(pt);
538 l_weights.push_back(Pw[k]);
544 std::vector<vpImagePoint> v_crossingPoints;
545 l_crossingPoints.
front();
549 v_crossingPoints.push_back(pt);
550 l_crossingPoints.
next();
551 while (!l_crossingPoints.
outside()) {
552 s = l_crossingPoints.
value();
555 v_crossingPoints.push_back(pt);
558 l_crossingPoints.
next();
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);
575 std::vector<vpImagePoint> v_crossingPoints;
576 vpMeSite s = l_crossingPoints.front();
579 v_crossingPoints.push_back(pt);
580 std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
582 for (; it != l_crossingPoints.end(); ++it) {
585 v_crossingPoints.push_back(pt_tmp);
595 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
596 std::vector<double> &l_weights)
599 l_controlPoints.clear();
601 unsigned int m = (
unsigned int)l_crossingPoints.size() - 1;
604 for (
unsigned int k = 1; k <= m; k++)
605 d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
608 std::vector<double> ubar;
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);
615 for (
unsigned int k = 0; k <= l_p; k++)
616 l_knots.push_back(0.0);
618 d = (double)(m + 1) / (double)(l_n - l_p + 1);
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]);
626 for (
unsigned int k = 0; k <= l_p; k++)
627 l_knots.push_back(1.0);
630 std::vector<vpImagePoint> Rk;
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) {
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());
643 else if (span == l_p) {
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());
650 else if (span == l_n) {
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());
658 Rk.push_back(l_crossingPoints[k]);
664 for (
unsigned int i = 1; i <= m - 1; i++) {
665 unsigned int span =
findSpan(ubar[i], 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;
677 for (
unsigned int i = 0; i < l_n - 1; i++) {
679 for (
unsigned int k = 0; k < m - 1; k++)
680 sum = sum + A[k][i] * Rk[k].get_i();
683 for (
unsigned int k = 0; k < m - 1; k++)
684 sum = sum + A[k][i] * Rk[k].get_j();
687 for (
unsigned int k = 0; k < m - 1; k++)
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++) {
705 l_controlPoints.push_back(pt);
706 l_weights.push_back(Pw[k]);
708 l_controlPoints.push_back(l_crossingPoints[m]);
709 l_weights.push_back(1.0);
715 std::vector<vpImagePoint> v_crossingPoints;
716 l_crossingPoints.
front();
717 while (!l_crossingPoints.
outside()) {
720 v_crossingPoints.push_back(pt);
721 l_crossingPoints.
next();
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);
738 std::vector<vpImagePoint> v_crossingPoints;
739 for (std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
741 v_crossingPoints.push_back(pt);
Class that provides tools to compute and manipulate a B-Spline curve.
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.
std::vector< vpImagePoint > crossingPoints
Vector wich contains the points used during the interpolation method.
std::vector< double > knots
Vector which contain the knots .
static unsigned int findSpan(double l_u, unsigned int l_p, std::vector< double > &l_knots)
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(double ii, double jj)
Provide simple list management.
void next(void)
position the current element on the next one
void front(void)
Position the current element on the first element of the list.
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
type & value(void)
return the value of the current element
static double sqr(double x)
static long double comb(unsigned int n, unsigned int p)
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
void insert(const vpMatrix &A, unsigned int r, unsigned int c)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
double ifloat
Floating coordinates along i of a site.
double jfloat
Floating coordinates along j of a site.
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
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)
std::vector< double > weights
Vector which contains the weights associated to each control Points.
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)
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)
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)
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)
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)
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)