44 #include <visp/vpNurbs.h>
45 #include <visp/vpColVector.h>
55 double distancew = w1 -w2;
100 std::vector<double> &l_knots,
101 std::vector<vpImagePoint> &l_controlPoints,
102 std::vector<double> &l_weights)
104 vpBasisFunction* N = NULL;
111 for(
unsigned int j = 0; j <= l_p; j++)
113 ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i() * l_weights[l_i-l_p+j];
114 jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j() * l_weights[l_i-l_p+j];
115 wc = wc + N[j].value * l_weights[l_i-l_p+j];
121 if (N != NULL)
delete[] N;
135 vpBasisFunction* N = NULL;
142 for(
unsigned int j = 0; j <= p; j++)
144 ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i() *
weights[N[0].i+j];
145 jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j() *
weights[N[0].i+j];
146 wc = wc + N[j].value *
weights[N[0].i+j];
152 if (N != NULL)
delete[] N;
181 unsigned int l_p,
unsigned int l_der,
182 std::vector<double> &l_knots,
183 std::vector<vpImagePoint> &l_controlPoints,
184 std::vector<double> &l_weights)
187 vpBasisFunction** N = NULL;
190 for(
unsigned int k = 0; k <= l_der; k++)
192 derivate[k][0] = 0.0;
193 derivate[k][1] = 0.0;
194 derivate[k][2] = 0.0;
196 for(
unsigned int j = 0; j<= l_p; j++)
198 derivate[k][0] = derivate[k][0] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i();
199 derivate[k][1] = derivate[k][1] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j();
200 derivate[k][2] = derivate[k][2] + N[k][j].value*(l_weights[l_i-l_p+j]);
205 for(
unsigned int i = 0; i <= l_der; i++)
231 vpBasisFunction** N = NULL;
234 for(
unsigned int k = 0; k <= der; k++)
236 derivate[k][0] = 0.0;
237 derivate[k][1] = 0.0;
238 derivate[k][2] = 0.0;
239 for(
unsigned int j = 0; j<= p; j++)
241 derivate[k][0] = derivate[k][0] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i();
242 derivate[k][1] = derivate[k][1] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j();
243 derivate[k][2] = derivate[k][2] + N[k][j].value*(
weights[N[0][0].i-p+j]);
247 if (N!=NULL)
delete[] N;
270 unsigned int l_p,
unsigned int l_der,
271 std::vector<double> &l_knots,
272 std::vector<vpImagePoint> &l_controlPoints,
273 std::vector<double> &l_weights)
275 std::vector<vpImagePoint> A;
277 for(
unsigned int j = 0; j < l_controlPoints.size(); j++)
279 pt = l_controlPoints[j];
290 for(
unsigned int k = 0; k <= l_der; k++)
294 for(
unsigned int j = 1; j <= k; j++)
296 double tmpComb =
static_cast<double>(
vpMath::comb(k,j) );
297 ic = ic - tmpComb*Awders[k][2]*(CK[k-j].
get_i());
298 jc = jc - tmpComb*Awders[j][2]*(CK[k-j].
get_j());
300 CK[k].
set_ij(ic/Awders[0][2],jc/Awders[0][2]);
337 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)
340 std::vector<vpImagePoint>::iterator it1;
341 std::vector<double>::iterator it2;
345 for (
unsigned int j = 0; j <= l_p-l_s; j++)
347 Rw[j][0] = (l_controlPoints[l_k-l_p+j]).get_i() * l_weights[l_k-l_p+j];
348 Rw[j][1] = (l_controlPoints[l_k-l_p+j]).get_j() * l_weights[l_k-l_p+j];
349 Rw[j][2] = l_weights[l_k-l_p+j];
352 it1 = l_controlPoints.begin();
353 l_controlPoints.
insert(it1+(
int)l_k-(
int)l_s, l_r, pt);
354 it2 = l_weights.begin();
355 l_weights.insert(it2+(
int)l_k-(
int)l_s, l_r, w);
359 for (
unsigned int j = 1; j <= l_r; j++)
363 for (
unsigned int i = 0; i <=l_p-j-l_s; i++)
365 alpha = (l_u - l_knots[L+i])/(l_knots[i+l_k+1] - l_knots[L+i]);
366 Rw[i][0]= alpha*Rw[i+1][0]+(1.0-alpha)*Rw[i][0];
367 Rw[i][1]= alpha*Rw[i+1][1]+(1.0-alpha)*Rw[i][1];
368 Rw[i][2]= alpha*Rw[i+1][2]+(1.0-alpha)*Rw[i][2];
371 pt.
set_ij(Rw[0][0]/Rw[0][2],Rw[0][1]/Rw[0][2]);
372 l_controlPoints[L] = pt;
373 l_weights[L] = Rw[0][2];
375 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]);
376 l_controlPoints[l_k+l_r-j-l_s] = pt;
377 l_weights[l_k+l_r-j-l_s] = Rw[l_p-j-l_s][2];
380 for(
unsigned int j = L+1; j < l_k-l_s; j++)
382 pt.
set_ij(Rw[j-L][0]/Rw[j-L][2],Rw[j-L][1]/Rw[j-L][2]);
383 l_controlPoints[j] = pt;
384 l_weights[j] = Rw[j-L][2];
387 it2 = l_knots.begin();
388 l_knots.
insert(it2+(
int)l_k, l_r, l_u);
420 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)
422 unsigned int a =
findSpan(l_x[0], l_p, l_knots);
423 unsigned int b =
findSpan(l_x[l_r], l_p, l_knots);
426 unsigned int n = (
unsigned int)l_controlPoints.size();
427 unsigned int m = (
unsigned int)l_knots.size();
429 for (
unsigned int j = 0; j < n; j++)
431 l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
434 std::vector<double> l_knots_tmp(l_knots);
435 std::vector<vpImagePoint> l_controlPoints_tmp(l_controlPoints);
436 std::vector<double> l_weights_tmp(l_weights);
441 for (
unsigned int j = 0; j <= l_r; j++)
443 l_controlPoints.push_back(pt);
444 l_weights.push_back(w);
445 l_knots.push_back(w);
448 for(
unsigned int j = b+l_p; j <= m-1; j++) l_knots[j+l_r+1] = l_knots_tmp[j];
450 for (
unsigned int j = b-1; j <= n-1; j++)
452 l_controlPoints[j+l_r+1] = l_controlPoints_tmp[j];
453 l_weights[j+l_r+1] = l_weights_tmp[j];
456 unsigned int i = b+l_p-1;
457 unsigned int k = b+l_p+l_r;
460 unsigned int j = l_r + 1;
463 while(l_x[j] <= l_knots[i] && i > a)
465 l_controlPoints[k-l_p-1] = l_controlPoints_tmp[i-l_p-1];
466 l_weights[k-l_p-1] = l_weights_tmp[i-l_p-1];
467 l_knots[k] = l_knots_tmp[i];
472 l_controlPoints[k-l_p-1] = l_controlPoints[k-l_p];
473 l_weights[k-l_p-1] = l_weights[k-l_p];
475 for (
unsigned int l = 1; l <= l_p; l++)
477 unsigned int ind = k-l_p+l;
478 double alpha = l_knots[k+l] - l_x[j];
480 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
482 l_controlPoints[ind-1] = l_controlPoints[ind];
483 l_weights[ind-1] = l_weights[ind];
487 alpha = alpha/(l_knots[k+l]-l_knots_tmp[i-l_p+l]);
488 l_controlPoints[ind-1].
set_i( alpha * l_controlPoints[ind-1].get_i() + (1.0-alpha) * l_controlPoints[ind].get_i());
489 l_controlPoints[ind-1].set_j( alpha * l_controlPoints[ind-1].get_j() + (1.0-alpha) * l_controlPoints[ind].get_j());
490 l_weights[ind-1] = alpha * l_weights[ind-1] + (1.0-alpha) * l_weights[ind];
498 for (
unsigned int j = 0; j < n; j++)
500 l_controlPoints[j].set_ij(l_controlPoints[j].get_i()/l_weights[j],l_controlPoints[j].get_j()/l_weights[j]);
541 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)
543 unsigned int n = (
unsigned int)l_controlPoints.size();
544 unsigned int m = n + l_p + 1;
546 for (
unsigned int j = 0; j < n; j++)
548 l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
551 unsigned int ord = l_p + 1;
552 double fout = (2*l_r-l_s-l_p)/2;
553 unsigned int last = l_r - l_s;
554 unsigned int first = l_r - l_p;
555 unsigned int tblSize = 2*l_p+1;
557 double *tempW =
new double[tblSize];
562 unsigned int i, j, ii, jj;
564 for(t = 0; t < l_num; t++)
566 unsigned int off = first - 1;
567 tempP[0] = l_controlPoints[off];
568 tempW[0] = l_weights[off];
569 tempP[last+1-off] = l_controlPoints[last+1];
570 tempW[last+1-off] = l_weights[last+1];
578 alfi = (l_u - l_knots[i])/(l_knots[i+ord+t]-l_knots[i]);
579 alfj = (l_u - l_knots[j-t])/(l_knots[j+ord]-l_knots[j-t]);
580 pt.
set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
581 tempP[ii].
set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
582 tempP[ii].
set_j((l_controlPoints[i].get_j()-(1.0-alfi)*tempP[ii-1].get_j())/alfi);
583 tempW[ii] = ((l_weights[i]-(1.0-alfi)*tempW[ii-1])/alfi);
584 tempP[jj].
set_i((l_controlPoints[j].get_i()-alfj*tempP[jj+1].get_i())/(1.0-alfj));
585 tempP[jj].
set_j((l_controlPoints[j].get_j()-alfj*tempP[jj+1].get_j())/(1.0-alfj));
586 tempW[jj] = ((l_weights[j]-alfj*tempW[jj+1])/(1.0-alfj));
595 double distancei = tempP[ii-1].
get_i() - tempP[jj+1].
get_i();
596 double distancej = tempP[ii-1].
get_j() - tempP[jj+1].
get_j();
597 double distancew = tempW[ii-1] - tempW[jj+1];
599 if(distance <= l_TOL) remflag = 1;
603 alfi = (l_u - l_knots[i])/(l_knots[i+ord+t]-l_knots[i]);
604 double distancei = l_controlPoints[i].get_i() - (alfi*tempP[ii+t+1].
get_i()+(1.0-alfi)*tempP[ii-1].get_i());
605 double distancej = l_controlPoints[i].get_j() - (alfi*tempP[ii+t+1].
get_j()+(1.0-alfi)*tempP[ii-1].get_j());
606 double distancew = l_weights[i] - (alfi*tempW[ii+t+1]+(1.0-alfi)*tempW[ii-1]);
608 if(distance <= l_TOL) remflag = 1;
610 if (remflag == 0)
break;
617 l_controlPoints[i].set_i(tempP[i-off].get_i());
618 l_controlPoints[i].set_j(tempP[i-off].get_j());
619 l_weights[i] = tempW[i-off];
620 l_controlPoints[j].set_i(tempP[j-off].get_i());
621 l_controlPoints[j].set_j(tempP[j-off].get_j());
622 l_weights[j] = tempW[j-off];
635 for(
unsigned int k = l_r+1; k <= m; k++) l_knots[k-t] = l_knots[k];
636 j = (
unsigned int)fout;
638 for(
unsigned int k = 1; k< t; k++)
643 for(
unsigned int k = i+1; k <= n; k++)
645 l_controlPoints[j].
set_i(l_controlPoints[k].get_i());
646 l_controlPoints[j].set_j(l_controlPoints[k].get_j());
647 l_weights[j] = l_weights[k];
650 for(
unsigned int k = 0; k < t; k++)
652 l_knots.erase(l_knots.end()-1);
653 l_controlPoints.erase(l_controlPoints.end()-1);
656 for(
unsigned int k = 0; k < l_controlPoints.size(); k++)
657 l_controlPoints[k].set_ij(l_controlPoints[k].get_i()/l_weights[k],l_controlPoints[k].get_j()/l_weights[k]);
699 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)
702 l_controlPoints.clear();
704 unsigned int n = (
unsigned int)l_crossingPoints.size()-1;
705 unsigned int m = n+l_p+1;
708 for(
unsigned int k=1; k<=n; k++)
709 d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
712 std::vector<double> ubar;
714 for(
unsigned int k=1; k<n; k++)
715 ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
719 for(
unsigned int k = 0; k <= l_p; k++)
720 l_knots.push_back(0.0);
723 for(
unsigned int k = 1; k <= l_p; k++)
727 for(
unsigned int k = 1; k <= n-l_p; k++)
729 l_knots.push_back(sum/l_p);
730 sum = sum - ubar[k-1] + ubar[l_p+k-1];
733 for(
unsigned int k = m-l_p; k <= m; k++)
734 l_knots.push_back(1.0);
739 for(
unsigned int i = 0; i <= n; i++)
741 unsigned int span =
findSpan(ubar[i], l_p, l_knots);
743 for (
unsigned int k = 0; k <= l_p; k++) A[i][span-l_p+k] = N[k].value;
752 for (
unsigned int k = 0; k <= n; k++)
754 Qi[k] = l_crossingPoints[k].get_i();
755 Qj[k] = l_crossingPoints[k].get_j();
763 for (
unsigned int k = 0; k <= n; k++)
766 l_controlPoints.push_back(pt);
767 l_weights.push_back(Pw[k]);
780 std::vector<vpImagePoint> v_crossingPoints;
781 l_crossingPoints.
front();
785 v_crossingPoints.push_back(pt);
786 l_crossingPoints.
next();
787 while(!l_crossingPoints.
outside())
793 v_crossingPoints.push_back(pt);
796 l_crossingPoints.
next();
810 std::vector<vpImagePoint> v_crossingPoints;
811 for(std::list<vpImagePoint>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
812 v_crossingPoints.push_back(*it);
827 std::vector<vpImagePoint> v_crossingPoints;
828 vpMeSite s = l_crossingPoints.front();
831 v_crossingPoints.push_back(pt);
832 std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
834 for(; it!=l_crossingPoints.end(); ++it){
837 v_crossingPoints.push_back(pt_tmp);
869 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)
872 l_controlPoints.clear();
874 unsigned int m = (
unsigned int)l_crossingPoints.size()-1;
877 for(
unsigned int k=1; k<=m; k++)
878 d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
881 std::vector<double> ubar;
883 for(
unsigned int k=1; k<m; k++)
884 ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
888 for(
unsigned int k = 0; k <= l_p; k++)
889 l_knots.push_back(0.0);
891 d = (double)(m+1)/(double)(l_n-l_p+1);
895 for(
unsigned int j = 1; j <= l_n-l_p; j++)
899 l_knots.push_back((1.0-alpha)*ubar[(
unsigned int)i-1]+alpha*ubar[(
unsigned int)i]);
902 for(
unsigned int k = 0; k <= l_p ; k++)
903 l_knots.push_back(1.0);
906 std::vector<vpImagePoint> Rk;
908 for(
unsigned int k = 1; k <= m-1; k++)
910 unsigned int span =
findSpan(ubar[k], l_p, l_knots);
911 if (span == l_p && span == l_n)
914 vpImagePoint pt(l_crossingPoints[k].get_i()-N[0].value*l_crossingPoints[0].get_i()-N[l_p].value*l_crossingPoints[m].get_i(),
915 l_crossingPoints[k].get_j()-N[0].value*l_crossingPoints[0].get_j()-N[l_p].value*l_crossingPoints[m].get_j());
919 else if (span == l_p)
922 vpImagePoint pt(l_crossingPoints[k].get_i()-N[0].value*l_crossingPoints[0].get_i(),
923 l_crossingPoints[k].get_j()-N[0].value*l_crossingPoints[0].get_j());
927 else if (span == l_n)
930 vpImagePoint pt(l_crossingPoints[k].get_i()-N[l_p].value*l_crossingPoints[m].get_i(),
931 l_crossingPoints[k].get_j()-N[l_p].value*l_crossingPoints[m].get_j());
937 Rk.push_back(l_crossingPoints[k]);
943 for(
unsigned int i = 1; i <= m-1; i++)
945 unsigned int span =
findSpan(ubar[i], l_p, l_knots);
947 for (
unsigned int k = 0; k <= l_p; k++)
949 if (N[k].i > 0 && N[k].i < l_n)
950 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);
1081 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
1093 std::vector<vpImagePoint> v_crossingPoints;
1094 l_crossingPoints.
front();
1095 while(!l_crossingPoints.
outside())
1097 v_crossingPoints.push_back(l_crossingPoints.
value());
1098 l_crossingPoints.
next();
1117 std::vector<vpImagePoint> v_crossingPoints;
1118 l_crossingPoints.
front();
1119 while(!l_crossingPoints.
outside())
1121 v_crossingPoints.push_back(l_crossingPoints.
value());
1122 l_crossingPoints.
next();
void set_j(const double j)
Definition of the vpMatrix class.
bool outside(void) const
Test if the current element is outside the list (on the virtual element)
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'...
void set_i(const double i)
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 insert(const vpMatrix &A, const unsigned int r, const unsigned int c)
void set_ij(const double i, const double j)
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)
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)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Class that provides tools to compute and manipulate a B-Spline curve.
std::vector< double > weights
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Compute the pseudo inverse of the matrix using the SVD.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
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)