40 #include <visp3/me/vpNurbs.h> 41 #include <visp3/core/vpColVector.h> 51 double distancew = w1 -w2;
97 std::vector<double> &l_knots,
98 std::vector<vpImagePoint> &l_controlPoints,
99 std::vector<double> &l_weights)
101 vpBasisFunction* N = NULL;
108 for(
unsigned int j = 0; j <= l_p; j++)
110 ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i() * l_weights[l_i-l_p+j];
111 jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j() * l_weights[l_i-l_p+j];
112 wc = wc + N[j].value * l_weights[l_i-l_p+j];
118 if (N != NULL)
delete[] N;
132 vpBasisFunction* N = NULL;
139 for(
unsigned int j = 0; j <= p; j++)
141 ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i() *
weights[N[0].i+j];
142 jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j() *
weights[N[0].i+j];
143 wc = wc + N[j].value *
weights[N[0].i+j];
149 if (N != NULL)
delete[] N;
178 unsigned int l_p,
unsigned int l_der,
179 std::vector<double> &l_knots,
180 std::vector<vpImagePoint> &l_controlPoints,
181 std::vector<double> &l_weights)
184 vpBasisFunction** N = NULL;
187 for(
unsigned int k = 0; k <= l_der; k++)
189 derivate[k][0] = 0.0;
190 derivate[k][1] = 0.0;
191 derivate[k][2] = 0.0;
193 for(
unsigned int j = 0; j<= l_p; j++)
195 derivate[k][0] = derivate[k][0] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i();
196 derivate[k][1] = derivate[k][1] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j();
197 derivate[k][2] = derivate[k][2] + N[k][j].value*(l_weights[l_i-l_p+j]);
202 for(
unsigned int i = 0; i <= l_der; i++)
228 vpBasisFunction** N = NULL;
231 for(
unsigned int k = 0; k <= der; k++)
233 derivate[k][0] = 0.0;
234 derivate[k][1] = 0.0;
235 derivate[k][2] = 0.0;
236 for(
unsigned int j = 0; j<= p; j++)
238 derivate[k][0] = derivate[k][0] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i();
239 derivate[k][1] = derivate[k][1] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j();
240 derivate[k][2] = derivate[k][2] + N[k][j].value*(
weights[N[0][0].i-p+j]);
244 if (N!=NULL)
delete[] N;
267 unsigned int l_p,
unsigned int l_der,
268 std::vector<double> &l_knots,
269 std::vector<vpImagePoint> &l_controlPoints,
270 std::vector<double> &l_weights)
272 std::vector<vpImagePoint> A;
274 for(
unsigned int j = 0; j < l_controlPoints.size(); j++)
276 pt = l_controlPoints[j];
286 for(
unsigned int k = 0; k <= l_der; k++)
288 double ic = Awders[k][0];
289 double jc = Awders[k][1];
290 for(
unsigned int j = 1; j <= k; j++)
292 double tmpComb =
static_cast<double>(
vpMath::comb(k,j) );
293 ic = ic - tmpComb*Awders[k][2]*(CK[k-j].
get_i());
294 jc = jc - tmpComb*Awders[j][2]*(CK[k-j].
get_j());
296 CK[k].
set_ij(ic/Awders[0][2],jc/Awders[0][2]);
333 void vpNurbs::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)
336 std::vector<vpImagePoint>::iterator it1;
337 std::vector<double>::iterator it2;
341 for (
unsigned int j = 0; j <= l_p-l_s; j++)
343 Rw[j][0] = (l_controlPoints[l_k-l_p+j]).get_i() * l_weights[l_k-l_p+j];
344 Rw[j][1] = (l_controlPoints[l_k-l_p+j]).get_j() * l_weights[l_k-l_p+j];
345 Rw[j][2] = l_weights[l_k-l_p+j];
348 it1 = l_controlPoints.begin();
349 l_controlPoints.
insert(it1+(
int)l_k-(
int)l_s, l_r, pt);
350 it2 = l_weights.begin();
351 l_weights.insert(it2+(
int)l_k-(
int)l_s, l_r, w);
355 for (
unsigned int j = 1; j <= l_r; j++)
359 for (
unsigned int i = 0; i <=l_p-j-l_s; i++)
361 alpha = (l_u - l_knots[L+i])/(l_knots[i+l_k+1] - l_knots[L+i]);
362 Rw[i][0]= alpha*Rw[i+1][0]+(1.0-alpha)*Rw[i][0];
363 Rw[i][1]= alpha*Rw[i+1][1]+(1.0-alpha)*Rw[i][1];
364 Rw[i][2]= alpha*Rw[i+1][2]+(1.0-alpha)*Rw[i][2];
367 pt.
set_ij(Rw[0][0]/Rw[0][2],Rw[0][1]/Rw[0][2]);
368 l_controlPoints[L] = pt;
369 l_weights[L] = Rw[0][2];
371 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]);
372 l_controlPoints[l_k+l_r-j-l_s] = pt;
373 l_weights[l_k+l_r-j-l_s] = Rw[l_p-j-l_s][2];
376 for(
unsigned int j = L+1; j < l_k-l_s; j++)
378 pt.
set_ij(Rw[j-L][0]/Rw[j-L][2],Rw[j-L][1]/Rw[j-L][2]);
379 l_controlPoints[j] = pt;
380 l_weights[j] = Rw[j-L][2];
383 it2 = l_knots.begin();
384 l_knots.
insert(it2+(
int)l_k, l_r, l_u);
416 void vpNurbs::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)
418 unsigned int a =
findSpan(l_x[0], l_p, l_knots);
419 unsigned int b =
findSpan(l_x[l_r], l_p, l_knots);
422 unsigned int n = (
unsigned int)l_controlPoints.size();
423 unsigned int m = (
unsigned int)l_knots.size();
425 for (
unsigned int j = 0; j < n; j++)
427 l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
430 std::vector<double> l_knots_tmp(l_knots);
431 std::vector<vpImagePoint> l_controlPoints_tmp(l_controlPoints);
432 std::vector<double> l_weights_tmp(l_weights);
437 for (
unsigned int j = 0; j <= l_r; j++)
439 l_controlPoints.push_back(pt);
440 l_weights.push_back(w);
441 l_knots.push_back(w);
444 for(
unsigned int j = b+l_p; j <= m-1; j++) l_knots[j+l_r+1] = l_knots_tmp[j];
446 for (
unsigned int j = b-1; j <= n-1; j++)
448 l_controlPoints[j+l_r+1] = l_controlPoints_tmp[j];
449 l_weights[j+l_r+1] = l_weights_tmp[j];
452 unsigned int i = b+l_p-1;
453 unsigned int k = b+l_p+l_r;
456 unsigned int j = l_r + 1;
459 while(l_x[j] <= l_knots[i] && i > a)
461 l_controlPoints[k-l_p-1] = l_controlPoints_tmp[i-l_p-1];
462 l_weights[k-l_p-1] = l_weights_tmp[i-l_p-1];
463 l_knots[k] = l_knots_tmp[i];
468 l_controlPoints[k-l_p-1] = l_controlPoints[k-l_p];
469 l_weights[k-l_p-1] = l_weights[k-l_p];
471 for (
unsigned int l = 1; l <= l_p; l++)
473 unsigned int ind = k-l_p+l;
474 double alpha = l_knots[k+l] - l_x[j];
476 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
478 l_controlPoints[ind-1] = l_controlPoints[ind];
479 l_weights[ind-1] = l_weights[ind];
483 alpha = alpha/(l_knots[k+l]-l_knots_tmp[i-l_p+l]);
484 l_controlPoints[ind-1].
set_i( alpha * l_controlPoints[ind-1].get_i() + (1.0-alpha) * l_controlPoints[ind].get_i());
485 l_controlPoints[ind-1].set_j( alpha * l_controlPoints[ind-1].get_j() + (1.0-alpha) * l_controlPoints[ind].get_j());
486 l_weights[ind-1] = alpha * l_weights[ind-1] + (1.0-alpha) * l_weights[ind];
494 for (
unsigned int j = 0; j < n; j++)
496 l_controlPoints[j].set_ij(l_controlPoints[j].get_i()/l_weights[j],l_controlPoints[j].get_j()/l_weights[j]);
537 vpNurbs::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)
539 unsigned int n = (
unsigned int)l_controlPoints.size();
540 unsigned int m = n + l_p + 1;
542 for (
unsigned int j = 0; j < n; j++)
544 l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
547 unsigned int ord = l_p + 1;
548 double fout = (2*l_r-l_s-l_p)/2.;
549 unsigned int last = l_r - l_s;
550 unsigned int first = l_r - l_p;
551 unsigned int tblSize = 2*l_p+1;
553 double *tempW =
new double[tblSize];
560 for(t = 0; t < l_num; t++)
562 unsigned int off = first - 1;
563 tempP[0] = l_controlPoints[off];
564 tempW[0] = l_weights[off];
565 tempP[last+1-off] = l_controlPoints[last+1];
566 tempW[last+1-off] = l_weights[last+1];
570 unsigned int jj = last -off;
574 alfi = (l_u - l_knots[i])/(l_knots[i+ord+t]-l_knots[i]);
575 alfj = (l_u - l_knots[j-t])/(l_knots[j+ord]-l_knots[j-t]);
576 pt.
set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
577 tempP[ii].
set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
578 tempP[ii].
set_j((l_controlPoints[i].get_j()-(1.0-alfi)*tempP[ii-1].get_j())/alfi);
579 tempW[ii] = ((l_weights[i]-(1.0-alfi)*tempW[ii-1])/alfi);
580 tempP[jj].
set_i((l_controlPoints[j].get_i()-alfj*tempP[jj+1].get_i())/(1.0-alfj));
581 tempP[jj].
set_j((l_controlPoints[j].get_j()-alfj*tempP[jj+1].get_j())/(1.0-alfj));
582 tempW[jj] = ((l_weights[j]-alfj*tempW[jj+1])/(1.0-alfj));
591 double distancei = tempP[ii-1].
get_i() - tempP[jj+1].
get_i();
592 double distancej = tempP[ii-1].
get_j() - tempP[jj+1].
get_j();
593 double distancew = tempW[ii-1] - tempW[jj+1];
595 if(distance <= l_TOL) remflag = 1;
599 alfi = (l_u - l_knots[i])/(l_knots[i+ord+t]-l_knots[i]);
600 double distancei = l_controlPoints[i].get_i() - (alfi*tempP[ii+t+1].
get_i()+(1.0-alfi)*tempP[ii-1].get_i());
601 double distancej = l_controlPoints[i].get_j() - (alfi*tempP[ii+t+1].
get_j()+(1.0-alfi)*tempP[ii-1].get_j());
602 double distancew = l_weights[i] - (alfi*tempW[ii+t+1]+(1.0-alfi)*tempW[ii-1]);
604 if(distance <= l_TOL) remflag = 1;
606 if (remflag == 0)
break;
613 l_controlPoints[i].set_i(tempP[i-off].get_i());
614 l_controlPoints[i].set_j(tempP[i-off].get_j());
615 l_weights[i] = tempW[i-off];
616 l_controlPoints[j].set_i(tempP[j-off].get_i());
617 l_controlPoints[j].set_j(tempP[j-off].get_j());
618 l_weights[j] = tempW[j-off];
631 for(
unsigned int k = l_r+1; k <= m; k++) l_knots[k-t] = l_knots[k];
632 j = (
unsigned int)fout;
634 for(
unsigned int k = 1; k< t; k++)
639 for(
unsigned int k = i+1; k <= n; k++)
641 l_controlPoints[j].
set_i(l_controlPoints[k].get_i());
642 l_controlPoints[j].set_j(l_controlPoints[k].get_j());
643 l_weights[j] = l_weights[k];
646 for(
unsigned int k = 0; k < t; k++)
648 l_knots.erase(l_knots.end()-1);
649 l_controlPoints.erase(l_controlPoints.end()-1);
652 for(
unsigned int k = 0; k < l_controlPoints.size(); k++)
653 l_controlPoints[k].set_ij(l_controlPoints[k].get_i()/l_weights[k],l_controlPoints[k].get_j()/l_weights[k]);
695 void vpNurbs::globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints,
unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
700 "Bad degree of the NURBS basis functions")) ;
704 l_controlPoints.clear();
706 unsigned int n = (
unsigned int)l_crossingPoints.size()-1;
707 unsigned int m = n+l_p+1;
710 for(
unsigned int k=1; k<=n; k++)
711 d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
714 std::vector<double> ubar;
716 for(
unsigned int k=1; k<n; k++) {
717 ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
722 for(
unsigned int k = 0; k <= l_p; k++)
723 l_knots.push_back(0.0);
726 for(
unsigned int k = 1; k <= l_p; k++)
730 for(
unsigned int k = 1; k <= n-l_p; k++)
732 l_knots.push_back(sum/l_p);
733 sum = sum - ubar[k-1] + ubar[l_p+k-1];
736 for(
unsigned int k = m-l_p; k <= m; k++)
737 l_knots.push_back(1.0);
742 for(
unsigned int i = 0; i <= n; i++)
744 unsigned int span =
findSpan(ubar[i], l_p, l_knots);
746 for (
unsigned int k = 0; k <= l_p; k++) A[i][span-l_p+k] = N[k].value;
755 for (
unsigned int k = 0; k <= n; k++)
757 Qi[k] = l_crossingPoints[k].get_i();
758 Qj[k] = l_crossingPoints[k].get_j();
766 for (
unsigned int k = 0; k <= n; k++)
769 l_controlPoints.push_back(pt);
770 l_weights.push_back(Pw[k]);
783 std::vector<vpImagePoint> v_crossingPoints;
784 l_crossingPoints.
front();
788 v_crossingPoints.push_back(pt);
789 l_crossingPoints.
next();
790 while(!l_crossingPoints.
outside())
792 s = l_crossingPoints.
value();
796 v_crossingPoints.push_back(pt);
799 l_crossingPoints.
next();
813 std::vector<vpImagePoint> v_crossingPoints;
814 for(std::list<vpImagePoint>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
815 v_crossingPoints.push_back(*it);
830 std::vector<vpImagePoint> v_crossingPoints;
831 vpMeSite s = l_crossingPoints.front();
834 v_crossingPoints.push_back(pt);
835 std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
837 for(; it!=l_crossingPoints.end(); ++it){
840 v_crossingPoints.push_back(pt_tmp);
872 void vpNurbs::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)
875 l_controlPoints.clear();
877 unsigned int m = (
unsigned int)l_crossingPoints.size()-1;
880 for(
unsigned int k=1; k<=m; k++)
881 d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
884 std::vector<double> ubar;
886 for(
unsigned int k=1; k<m; k++)
887 ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
891 for(
unsigned int k = 0; k <= l_p; k++)
892 l_knots.push_back(0.0);
894 d = (double)(m+1)/(double)(l_n-l_p+1);
896 for(
unsigned int j = 1; j <= l_n-l_p; j++)
898 double i = floor(j*d);
899 double alpha = j*d-i;
900 l_knots.push_back((1.0-alpha)*ubar[(
unsigned int)i-1]+alpha*ubar[(
unsigned int)i]);
903 for(
unsigned int k = 0; k <= l_p ; k++)
904 l_knots.push_back(1.0);
907 std::vector<vpImagePoint> Rk;
909 for(
unsigned int k = 1; k <= m-1; k++)
911 unsigned int span =
findSpan(ubar[k], l_p, l_knots);
912 if (span == l_p && span == l_n)
915 vpImagePoint pt(l_crossingPoints[k].get_i()-N[0].value*l_crossingPoints[0].get_i()-N[l_p].value*l_crossingPoints[m].get_i(),
916 l_crossingPoints[k].get_j()-N[0].value*l_crossingPoints[0].get_j()-N[l_p].value*l_crossingPoints[m].get_j());
920 else if (span == l_p)
923 vpImagePoint pt(l_crossingPoints[k].get_i()-N[0].value*l_crossingPoints[0].get_i(),
924 l_crossingPoints[k].get_j()-N[0].value*l_crossingPoints[0].get_j());
928 else if (span == l_n)
931 vpImagePoint pt(l_crossingPoints[k].get_i()-N[l_p].value*l_crossingPoints[m].get_i(),
932 l_crossingPoints[k].get_j()-N[l_p].value*l_crossingPoints[m].get_j());
938 Rk.push_back(l_crossingPoints[k]);
944 for(
unsigned int i = 1; i <= m-1; i++)
946 unsigned int span =
findSpan(ubar[i], l_p, l_knots);
948 for (
unsigned int k = 0; k <= l_p; k++)
950 if (N[k].i > 0 && N[k].i < l_n)
951 A[i-1][N[k].i-1] = N[k].value;
959 for (
unsigned int i = 0; i < l_n-1; i++)
962 for (
unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]*Rk[k].get_i();
965 for (
unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]*Rk[k].get_j();
968 for (
unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i];
981 l_controlPoints.push_back(l_crossingPoints[0]);
982 l_weights.push_back(1.0);
983 for (
unsigned int k = 0; k < l_n-1; k++)
986 l_controlPoints.push_back(pt);
987 l_weights.push_back(Pw[k]);
989 l_controlPoints.push_back(l_crossingPoints[m]);
990 l_weights.push_back(1.0);
1011 std::vector<vpImagePoint> v_crossingPoints;
1012 l_crossingPoints.
front();
1013 while(!l_crossingPoints.
outside())
1017 v_crossingPoints.push_back(pt);
1018 l_crossingPoints.
next();
1035 std::vector<vpImagePoint> v_crossingPoints;
1036 for(std::list<vpImagePoint>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
1037 v_crossingPoints.push_back(*it);
1060 std::vector<vpImagePoint> v_crossingPoints;
1061 for(std::list<vpMeSite>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
1063 v_crossingPoints.push_back(pt);
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
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)
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)
Provide simple list management.
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'...
error that can be emited by ViSP classes.
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)
void next(void)
position the current element on the next one
static vpBasisFunction ** computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector< double > &l_knots)
static unsigned int findSpan(double l_u, unsigned int l_p, std::vector< double > &l_knots)
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)
void set_i(const double ii)
void insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
static double sqr(double x)
void front(void)
Position the current element on the first element of the list.
static vpBasisFunction * computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector< double > &l_knots)
type & value(void)
return the value of the current element
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)
void set_j(const double jj)
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 long double comb(unsigned int n, unsigned int p)
Implementation of column vector and the associated operations.
Class that provides tools to compute and manipulate a B-Spline curve.
std::vector< double > weights
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
void set_ij(const double ii, const double jj)
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)