38 #include <visp3/core/vpColVector.h>
39 #include <visp3/me/vpNurbs.h>
47 double distancew = w1 - w2;
83 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
85 vpBasisFunction *N = NULL;
92 for (
unsigned int j = 0; j <= l_p; j++) {
93 ic = ic + N[j].value * (l_controlPoints[l_i - l_p + j]).get_i() * l_weights[l_i - l_p + j];
94 jc = jc + N[j].value * (l_controlPoints[l_i - l_p + j]).get_j() * l_weights[l_i - l_p + j];
95 wc = wc + N[j].value * l_weights[l_i - l_p + j];
118 vpBasisFunction *N = NULL;
125 for (
unsigned int j = 0; j <= p; j++) {
126 ic = ic + N[j].value * (controlPoints[N[0].i + j]).get_i() *
weights[N[0].i + j];
127 jc = jc + N[j].value * (controlPoints[N[0].i + j]).get_j() *
weights[N[0].i + j];
128 wc = wc + N[j].value *
weights[N[0].i + j];
168 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
169 std::vector<double> &l_weights)
172 vpBasisFunction **N = NULL;
175 for (
unsigned int k = 0; k <= l_der; k++) {
176 derivate[k][0] = 0.0;
177 derivate[k][1] = 0.0;
178 derivate[k][2] = 0.0;
180 for (
unsigned int j = 0; j <= l_p; j++) {
181 derivate[k][0] = derivate[k][0] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_i();
182 derivate[k][1] = derivate[k][1] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_j();
183 derivate[k][2] = derivate[k][2] + N[k][j].value * (l_weights[l_i - l_p + j]);
188 for (
unsigned int i = 0; i <= l_der; i++)
220 vpBasisFunction **N = NULL;
223 for (
unsigned int k = 0; k <= der; k++) {
224 derivate[k][0] = 0.0;
225 derivate[k][1] = 0.0;
226 derivate[k][2] = 0.0;
227 for (
unsigned int j = 0; j <= p; j++) {
228 derivate[k][0] = derivate[k][0] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_i();
229 derivate[k][1] = derivate[k][1] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_j();
230 derivate[k][2] = derivate[k][2] + N[k][j].value * (
weights[N[0][0].i - p + j]);
258 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
259 std::vector<double> &l_weights)
261 std::vector<vpImagePoint> A;
263 for (
unsigned int j = 0; j < l_controlPoints.size(); j++) {
264 pt = l_controlPoints[j];
274 for (
unsigned int k = 0; k <= l_der; k++) {
275 double ic = Awders[k][0];
276 double jc = Awders[k][1];
277 for (
unsigned int j = 1; j <= k; j++) {
278 double tmpComb =
static_cast<double>(
vpMath::comb(k, j));
279 ic = ic - tmpComb * Awders[k][2] * (CK[k - j].
get_i());
280 jc = jc - tmpComb * Awders[j][2] * (CK[k - j].
get_j());
282 CK[k].
set_ij(ic / Awders[0][2], jc / Awders[0][2]);
323 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
324 std::vector<double> &l_weights)
327 std::vector<vpImagePoint>::iterator it1;
328 std::vector<double>::iterator it2;
332 for (
unsigned int j = 0; j <= l_p - l_s; j++) {
333 Rw[j][0] = (l_controlPoints[l_k - l_p + j]).get_i() * l_weights[l_k - l_p + j];
334 Rw[j][1] = (l_controlPoints[l_k - l_p + j]).get_j() * l_weights[l_k - l_p + j];
335 Rw[j][2] = l_weights[l_k - l_p + j];
338 it1 = l_controlPoints.begin();
339 l_controlPoints.
insert(it1 + (
int)l_k - (int)l_s, l_r, pt);
340 it2 = l_weights.begin();
341 l_weights.insert(it2 + (
int)l_k - (int)l_s, l_r, w);
345 for (
unsigned int j = 1; j <= l_r; j++) {
348 for (
unsigned int i = 0; i <= l_p - j - l_s; i++) {
349 alpha = (l_u - l_knots[L + i]) / (l_knots[i + l_k + 1] - l_knots[L + i]);
350 Rw[i][0] = alpha * Rw[i + 1][0] + (1.0 - alpha) * Rw[i][0];
351 Rw[i][1] = alpha * Rw[i + 1][1] + (1.0 - alpha) * Rw[i][1];
352 Rw[i][2] = alpha * Rw[i + 1][2] + (1.0 - alpha) * Rw[i][2];
355 pt.
set_ij(Rw[0][0] / Rw[0][2], Rw[0][1] / Rw[0][2]);
356 l_controlPoints[L] = pt;
357 l_weights[L] = Rw[0][2];
359 pt.
set_ij(Rw[l_p - j - l_s][0] / Rw[l_p - j - l_s][2], Rw[l_p - j - l_s][1] / Rw[l_p - j - l_s][2]);
360 l_controlPoints[l_k + l_r - j - l_s] = pt;
361 l_weights[l_k + l_r - j - l_s] = Rw[l_p - j - l_s][2];
364 for (
unsigned int j = L + 1; j < l_k - l_s; j++) {
365 pt.
set_ij(Rw[j - L][0] / Rw[j - L][2], Rw[j - L][1] / Rw[j - L][2]);
366 l_controlPoints[j] = pt;
367 l_weights[j] = Rw[j - L][2];
370 it2 = l_knots.begin();
371 l_knots.
insert(it2 + (
int)l_k, l_r, l_u);
405 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
407 unsigned int a =
findSpan(l_x[0], l_p, l_knots);
408 unsigned int b =
findSpan(l_x[l_r], l_p, l_knots);
411 unsigned int n = (
unsigned int)l_controlPoints.size();
412 unsigned int m = (
unsigned int)l_knots.size();
414 for (
unsigned int j = 0; j < n; j++) {
415 l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
418 std::vector<double> l_knots_tmp(l_knots);
419 std::vector<vpImagePoint> l_controlPoints_tmp(l_controlPoints);
420 std::vector<double> l_weights_tmp(l_weights);
425 for (
unsigned int j = 0; j <= l_r; j++) {
426 l_controlPoints.push_back(pt);
427 l_weights.push_back(w);
428 l_knots.push_back(w);
431 for (
unsigned int j = b + l_p; j <= m - 1; j++)
432 l_knots[j + l_r + 1] = l_knots_tmp[j];
434 for (
unsigned int j = b - 1; j <= n - 1; j++) {
435 l_controlPoints[j + l_r + 1] = l_controlPoints_tmp[j];
436 l_weights[j + l_r + 1] = l_weights_tmp[j];
439 unsigned int i = b + l_p - 1;
440 unsigned int k = b + l_p + l_r;
443 unsigned int j = l_r + 1;
446 while (l_x[j] <= l_knots[i] && i > a) {
447 l_controlPoints[k - l_p - 1] = l_controlPoints_tmp[i - l_p - 1];
448 l_weights[k - l_p - 1] = l_weights_tmp[i - l_p - 1];
449 l_knots[k] = l_knots_tmp[i];
454 l_controlPoints[k - l_p - 1] = l_controlPoints[k - l_p];
455 l_weights[k - l_p - 1] = l_weights[k - l_p];
457 for (
unsigned int l = 1; l <= l_p; l++) {
458 unsigned int ind = k - l_p + l;
459 double alpha = l_knots[k + l] - l_x[j];
461 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon()) {
462 l_controlPoints[ind - 1] = l_controlPoints[ind];
463 l_weights[ind - 1] = l_weights[ind];
465 alpha = alpha / (l_knots[k + l] - l_knots_tmp[i - l_p + l]);
466 l_controlPoints[ind - 1].
set_i(alpha * l_controlPoints[ind - 1].get_i() +
467 (1.0 - alpha) * l_controlPoints[ind].get_i());
468 l_controlPoints[ind - 1].set_j(alpha * l_controlPoints[ind - 1].get_j() +
469 (1.0 - alpha) * l_controlPoints[ind].get_j());
470 l_weights[ind - 1] = alpha * l_weights[ind - 1] + (1.0 - alpha) * l_weights[ind];
478 for (
unsigned int j = 0; j < n; j++) {
479 l_controlPoints[j].set_ij(l_controlPoints[j].get_i() / l_weights[j], l_controlPoints[j].get_j() / l_weights[j]);
524 unsigned int l_p, std::vector<double> &l_knots,
525 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
527 unsigned int n = (
unsigned int)l_controlPoints.size();
528 unsigned int m = n + l_p + 1;
530 for (
unsigned int j = 0; j < n; j++) {
531 l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
534 unsigned int ord = l_p + 1;
535 double fout = (2 * l_r - l_s - l_p) / 2.;
536 unsigned int last = l_r - l_s;
537 unsigned int first = l_r - l_p;
538 unsigned int tblSize = 2 * l_p + 1;
540 double *tempW =
new double[tblSize];
547 for (t = 0; t < l_num; t++) {
548 unsigned int off = first - 1;
549 tempP[0] = l_controlPoints[off];
550 tempW[0] = l_weights[off];
551 tempP[last + 1 - off] = l_controlPoints[last + 1];
552 tempW[last + 1 - off] = l_weights[last + 1];
556 unsigned int jj = last - off;
559 alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
560 alfj = (l_u - l_knots[j - t]) / (l_knots[j + ord] - l_knots[j - t]);
561 pt.
set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
562 tempP[ii].
set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
563 tempP[ii].
set_j((l_controlPoints[i].get_j() - (1.0 - alfi) * tempP[ii - 1].get_j()) / alfi);
564 tempW[ii] = ((l_weights[i] - (1.0 - alfi) * tempW[ii - 1]) / alfi);
565 tempP[jj].
set_i((l_controlPoints[j].get_i() - alfj * tempP[jj + 1].
get_i()) / (1.0 - alfj));
566 tempP[jj].
set_j((l_controlPoints[j].get_j() - alfj * tempP[jj + 1].
get_j()) / (1.0 - alfj));
567 tempW[jj] = ((l_weights[j] - alfj * tempW[jj + 1]) / (1.0 - alfj));
575 double distancei = tempP[ii - 1].
get_i() - tempP[jj + 1].
get_i();
576 double distancej = tempP[ii - 1].
get_j() - tempP[jj + 1].
get_j();
577 double distancew = tempW[ii - 1] - tempW[jj + 1];
579 if (distance <= l_TOL)
582 alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
584 l_controlPoints[i].get_i() - (alfi * tempP[ii + t + 1].
get_i() + (1.0 - alfi) * tempP[ii - 1].get_i());
586 l_controlPoints[i].get_j() - (alfi * tempP[ii + t + 1].
get_j() + (1.0 - alfi) * tempP[ii - 1].get_j());
587 double distancew = l_weights[i] - (alfi * tempW[ii + t + 1] + (1.0 - alfi) * tempW[ii - 1]);
589 if (distance <= l_TOL)
598 l_controlPoints[i].set_i(tempP[i - off].get_i());
599 l_controlPoints[i].set_j(tempP[i - off].get_j());
600 l_weights[i] = tempW[i - off];
601 l_controlPoints[j].set_i(tempP[j - off].get_i());
602 l_controlPoints[j].set_j(tempP[j - off].get_j());
603 l_weights[j] = tempW[j - off];
616 for (
unsigned int k = l_r + 1; k <= m; k++)
617 l_knots[k - t] = l_knots[k];
618 j = (
unsigned int)fout;
620 for (
unsigned int k = 1; k < t; k++) {
626 for (
unsigned int k = i + 1; k <= n; k++) {
627 l_controlPoints[j].
set_i(l_controlPoints[k].get_i());
628 l_controlPoints[j].set_j(l_controlPoints[k].get_j());
629 l_weights[j] = l_weights[k];
632 for (
unsigned int k = 0; k < t; k++) {
633 l_knots.erase(l_knots.end() - 1);
634 l_controlPoints.erase(l_controlPoints.end() - 1);
637 for (
unsigned int k = 0; k < l_controlPoints.size(); k++)
638 l_controlPoints[k].set_ij(l_controlPoints[k].get_i() / l_weights[k], l_controlPoints[k].get_j() / l_weights[k]);
683 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
684 std::vector<double> &l_weights)
692 l_controlPoints.clear();
694 unsigned int n = (
unsigned int)l_crossingPoints.size() - 1;
695 unsigned int m = n + l_p + 1;
698 for (
unsigned int k = 1; k <= n; k++)
699 d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
702 std::vector<double> ubar;
704 for (
unsigned int k = 1; k < n; k++) {
705 ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
710 for (
unsigned int k = 0; k <= l_p; k++)
711 l_knots.push_back(0.0);
714 for (
unsigned int k = 1; k <= l_p; k++)
718 for (
unsigned int k = 1; k <= n - l_p; k++) {
719 l_knots.push_back(sum / l_p);
720 sum = sum - ubar[k - 1] + ubar[l_p + k - 1];
723 for (
unsigned int k = m - l_p; k <= m; k++)
724 l_knots.push_back(1.0);
729 for (
unsigned int i = 0; i <= n; i++) {
730 unsigned int span =
findSpan(ubar[i], l_p, l_knots);
732 for (
unsigned int k = 0; k <= l_p; k++)
733 A[i][span - l_p + k] = N[k].value;
742 for (
unsigned int k = 0; k <= n; k++) {
743 Qi[k] = l_crossingPoints[k].get_i();
744 Qj[k] = l_crossingPoints[k].get_j();
752 for (
unsigned int k = 0; k <= n; k++) {
754 l_controlPoints.push_back(pt);
755 l_weights.push_back(Pw[k]);
771 std::vector<vpImagePoint> v_crossingPoints;
772 l_crossingPoints.
front();
776 v_crossingPoints.push_back(pt);
777 l_crossingPoints.
next();
778 while (!l_crossingPoints.
outside()) {
779 s = l_crossingPoints.
value();
782 v_crossingPoints.push_back(pt);
785 l_crossingPoints.
next();
802 std::vector<vpImagePoint> v_crossingPoints;
803 for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
804 v_crossingPoints.push_back(*it);
821 std::vector<vpImagePoint> v_crossingPoints;
822 vpMeSite s = l_crossingPoints.front();
825 v_crossingPoints.push_back(pt);
826 std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
828 for (; it != l_crossingPoints.end(); ++it) {
831 v_crossingPoints.push_back(pt_tmp);
864 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
865 std::vector<double> &l_weights)
868 l_controlPoints.clear();
870 unsigned int m = (
unsigned int)l_crossingPoints.size() - 1;
873 for (
unsigned int k = 1; k <= m; k++)
874 d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
877 std::vector<double> ubar;
879 for (
unsigned int k = 1; k < m; k++)
880 ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
884 for (
unsigned int k = 0; k <= l_p; k++)
885 l_knots.push_back(0.0);
887 d = (double)(m + 1) / (double)(l_n - l_p + 1);
889 for (
unsigned int j = 1; j <= l_n - l_p; j++) {
890 double i = floor(j * d);
891 double alpha = j * d - i;
892 l_knots.push_back((1.0 - alpha) * ubar[(
unsigned int)i - 1] + alpha * ubar[(
unsigned int)i]);
895 for (
unsigned int k = 0; k <= l_p; k++)
896 l_knots.push_back(1.0);
899 std::vector<vpImagePoint> Rk;
901 for (
unsigned int k = 1; k <= m - 1; k++) {
902 unsigned int span =
findSpan(ubar[k], l_p, l_knots);
903 if (span == l_p && span == l_n) {
905 vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i() -
906 N[l_p].value * l_crossingPoints[m].get_i(),
907 l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j() -
908 N[l_p].value * l_crossingPoints[m].get_j());
911 }
else if (span == l_p) {
913 vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i(),
914 l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j());
917 }
else if (span == l_n) {
919 vpImagePoint pt(l_crossingPoints[k].get_i() - N[l_p].value * l_crossingPoints[m].get_i(),
920 l_crossingPoints[k].get_j() - N[l_p].value * l_crossingPoints[m].get_j());
924 Rk.push_back(l_crossingPoints[k]);
930 for (
unsigned int i = 1; i <= m - 1; i++) {
931 unsigned int span =
findSpan(ubar[i], l_p, l_knots);
933 for (
unsigned int k = 0; k <= l_p; k++) {
934 if (N[k].i > 0 && N[k].i < l_n)
935 A[i - 1][N[k].i - 1] = N[k].value;
943 for (
unsigned int i = 0; i < l_n - 1; i++) {
945 for (
unsigned int k = 0; k < m - 1; k++)
946 sum = sum + A[k][i] * Rk[k].get_i();
949 for (
unsigned int k = 0; k < m - 1; k++)
950 sum = sum + A[k][i] * Rk[k].get_j();
953 for (
unsigned int k = 0; k < m - 1; k++)
967 l_controlPoints.push_back(l_crossingPoints[0]);
968 l_weights.push_back(1.0);
969 for (
unsigned int k = 0; k < l_n - 1; k++) {
971 l_controlPoints.push_back(pt);
972 l_weights.push_back(Pw[k]);
974 l_controlPoints.push_back(l_crossingPoints[m]);
975 l_weights.push_back(1.0);
996 std::vector<vpImagePoint> v_crossingPoints;
997 l_crossingPoints.
front();
998 while (!l_crossingPoints.
outside()) {
1001 v_crossingPoints.push_back(pt);
1002 l_crossingPoints.
next();
1022 std::vector<vpImagePoint> v_crossingPoints;
1023 for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
1024 v_crossingPoints.push_back(*it);
1047 std::vector<vpImagePoint> v_crossingPoints;
1048 for (std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
1050 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)
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 emited 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'....
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
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)