36 #include <visp3/core/vpColVector.h>
37 #include <visp3/me/vpNurbs.h>
45 double distancew = w1 - w2;
54 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
56 vpBasisFunction *N =
nullptr;
63 for (
unsigned int j = 0; j <= l_p; j++) {
64 ic = ic + N[j].value * (l_controlPoints[l_i - l_p + j]).get_i() * l_weights[l_i - l_p + j];
65 jc = jc + N[j].value * (l_controlPoints[l_i - l_p + j]).get_j() * l_weights[l_i - l_p + j];
66 wc = wc + N[j].value * l_weights[l_i - l_p + j];
80 vpBasisFunction *N =
nullptr;
87 for (
unsigned int j = 0; j <= p; j++) {
88 ic = ic + N[j].value * (controlPoints[N[0].i + j]).get_i() *
weights[N[0].i + j];
89 jc = jc + N[j].value * (controlPoints[N[0].i + j]).get_j() *
weights[N[0].i + j];
90 wc = wc + N[j].value *
weights[N[0].i + j];
103 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
104 std::vector<double> &l_weights)
107 vpBasisFunction **N =
nullptr;
110 for (
unsigned int k = 0; k <= l_der; k++) {
111 derivate[k][0] = 0.0;
112 derivate[k][1] = 0.0;
113 derivate[k][2] = 0.0;
115 for (
unsigned int j = 0; j <= l_p; j++) {
116 derivate[k][0] = derivate[k][0] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_i();
117 derivate[k][1] = derivate[k][1] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_j();
118 derivate[k][2] = derivate[k][2] + N[k][j].value * (l_weights[l_i - l_p + j]);
123 for (
unsigned int i = 0; i <= l_der; i++)
133 vpBasisFunction **N =
nullptr;
136 for (
unsigned int k = 0; k <= der; k++) {
137 derivate[k][0] = 0.0;
138 derivate[k][1] = 0.0;
139 derivate[k][2] = 0.0;
140 for (
unsigned int j = 0; j <= p; j++) {
141 derivate[k][0] = derivate[k][0] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_i();
142 derivate[k][1] = derivate[k][1] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_j();
143 derivate[k][2] = derivate[k][2] + N[k][j].value * (
weights[N[0][0].i - p + j]);
154 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
155 std::vector<double> &l_weights)
157 std::vector<vpImagePoint> A;
159 for (
unsigned int j = 0; j < l_controlPoints.size(); j++) {
160 pt = l_controlPoints[j];
170 for (
unsigned int k = 0; k <= l_der; k++) {
171 double ic = Awders[k][0];
172 double jc = Awders[k][1];
173 for (
unsigned int j = 1; j <= k; j++) {
174 double tmpComb =
static_cast<double>(
vpMath::comb(k, j));
175 ic = ic - tmpComb * Awders[k][2] * (CK[k - j].
get_i());
176 jc = jc - tmpComb * Awders[j][2] * (CK[k - j].
get_j());
178 CK[k].
set_ij(ic / Awders[0][2], jc / Awders[0][2]);
192 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
193 std::vector<double> &l_weights)
196 std::vector<vpImagePoint>::iterator it1;
197 std::vector<double>::iterator it2;
201 for (
unsigned int j = 0; j <= l_p - l_s; j++) {
202 Rw[j][0] = (l_controlPoints[l_k - l_p + j]).get_i() * l_weights[l_k - l_p + j];
203 Rw[j][1] = (l_controlPoints[l_k - l_p + j]).get_j() * l_weights[l_k - l_p + j];
204 Rw[j][2] = l_weights[l_k - l_p + j];
207 it1 = l_controlPoints.begin();
208 l_controlPoints.
insert(it1 + (
int)l_k - (int)l_s, l_r, pt);
209 it2 = l_weights.begin();
210 l_weights.insert(it2 + (
int)l_k - (int)l_s, l_r, w);
214 for (
unsigned int j = 1; j <= l_r; j++) {
217 for (
unsigned int i = 0; i <= l_p - j - l_s; i++) {
218 alpha = (l_u - l_knots[L + i]) / (l_knots[i + l_k + 1] - l_knots[L + i]);
219 Rw[i][0] = alpha * Rw[i + 1][0] + (1.0 - alpha) * Rw[i][0];
220 Rw[i][1] = alpha * Rw[i + 1][1] + (1.0 - alpha) * Rw[i][1];
221 Rw[i][2] = alpha * Rw[i + 1][2] + (1.0 - alpha) * Rw[i][2];
224 pt.
set_ij(Rw[0][0] / Rw[0][2], Rw[0][1] / Rw[0][2]);
225 l_controlPoints[L] = pt;
226 l_weights[L] = Rw[0][2];
228 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]);
229 l_controlPoints[l_k + l_r - j - l_s] = pt;
230 l_weights[l_k + l_r - j - l_s] = Rw[l_p - j - l_s][2];
233 for (
unsigned int j = L + 1; j < l_k - l_s; j++) {
234 pt.
set_ij(Rw[j - L][0] / Rw[j - L][2], Rw[j - L][1] / Rw[j - L][2]);
235 l_controlPoints[j] = pt;
236 l_weights[j] = Rw[j - L][2];
239 it2 = l_knots.begin();
240 l_knots.
insert(it2 + (
int)l_k, l_r, l_u);
251 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
253 unsigned int a =
findSpan(l_x[0], l_p, l_knots);
254 unsigned int b =
findSpan(l_x[l_r], l_p, l_knots);
257 unsigned int n = (
unsigned int)l_controlPoints.size();
258 unsigned int m = (
unsigned int)l_knots.size();
260 for (
unsigned int j = 0; j < n; j++) {
261 l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
264 std::vector<double> l_knots_tmp(l_knots);
265 std::vector<vpImagePoint> l_controlPoints_tmp(l_controlPoints);
266 std::vector<double> l_weights_tmp(l_weights);
271 for (
unsigned int j = 0; j <= l_r; j++) {
272 l_controlPoints.push_back(pt);
273 l_weights.push_back(w);
274 l_knots.push_back(w);
277 for (
unsigned int j = b + l_p; j <= m - 1; j++)
278 l_knots[j + l_r + 1] = l_knots_tmp[j];
280 for (
unsigned int j = b - 1; j <= n - 1; j++) {
281 l_controlPoints[j + l_r + 1] = l_controlPoints_tmp[j];
282 l_weights[j + l_r + 1] = l_weights_tmp[j];
285 unsigned int i = b + l_p - 1;
286 unsigned int k = b + l_p + l_r;
289 unsigned int j = l_r + 1;
292 while (l_x[j] <= l_knots[i] && i > a) {
293 l_controlPoints[k - l_p - 1] = l_controlPoints_tmp[i - l_p - 1];
294 l_weights[k - l_p - 1] = l_weights_tmp[i - l_p - 1];
295 l_knots[k] = l_knots_tmp[i];
300 l_controlPoints[k - l_p - 1] = l_controlPoints[k - l_p];
301 l_weights[k - l_p - 1] = l_weights[k - l_p];
303 for (
unsigned int l = 1; l <= l_p; l++) {
304 unsigned int ind = k - l_p + l;
305 double alpha = l_knots[k + l] - l_x[j];
307 if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon()) {
308 l_controlPoints[ind - 1] = l_controlPoints[ind];
309 l_weights[ind - 1] = l_weights[ind];
312 alpha = alpha / (l_knots[k + l] - l_knots_tmp[i - l_p + l]);
313 l_controlPoints[ind - 1].
set_i(alpha * l_controlPoints[ind - 1].get_i() +
314 (1.0 - alpha) * l_controlPoints[ind].get_i());
315 l_controlPoints[ind - 1].set_j(alpha * l_controlPoints[ind - 1].get_j() +
316 (1.0 - alpha) * l_controlPoints[ind].get_j());
317 l_weights[ind - 1] = alpha * l_weights[ind - 1] + (1.0 - alpha) * l_weights[ind];
325 for (
unsigned int j = 0; j < n; j++) {
326 l_controlPoints[j].set_ij(l_controlPoints[j].get_i() / l_weights[j], l_controlPoints[j].get_j() / l_weights[j]);
336 unsigned int l_p, std::vector<double> &l_knots,
337 std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
339 unsigned int n = (
unsigned int)l_controlPoints.size();
340 unsigned int m = n + l_p + 1;
342 for (
unsigned int j = 0; j < n; j++) {
343 l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
346 unsigned int ord = l_p + 1;
347 double fout = (2 * l_r - l_s - l_p) / 2.;
348 unsigned int last = l_r - l_s;
349 unsigned int first = l_r - l_p;
350 unsigned int tblSize = 2 * l_p + 1;
352 double *tempW =
new double[tblSize];
359 for (t = 0; t < l_num; t++) {
360 unsigned int off = first - 1;
361 tempP[0] = l_controlPoints[off];
362 tempW[0] = l_weights[off];
363 tempP[last + 1 - off] = l_controlPoints[last + 1];
364 tempW[last + 1 - off] = l_weights[last + 1];
368 unsigned int jj = last - off;
371 alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
372 alfj = (l_u - l_knots[j - t]) / (l_knots[j + ord] - l_knots[j - t]);
373 pt.
set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
374 tempP[ii].
set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
375 tempP[ii].
set_j((l_controlPoints[i].get_j() - (1.0 - alfi) * tempP[ii - 1].get_j()) / alfi);
376 tempW[ii] = ((l_weights[i] - (1.0 - alfi) * tempW[ii - 1]) / alfi);
377 tempP[jj].
set_i((l_controlPoints[j].get_i() - alfj * tempP[jj + 1].
get_i()) / (1.0 - alfj));
378 tempP[jj].
set_j((l_controlPoints[j].get_j() - alfj * tempP[jj + 1].
get_j()) / (1.0 - alfj));
379 tempW[jj] = ((l_weights[j] - alfj * tempW[jj + 1]) / (1.0 - alfj));
387 double distancei = tempP[ii - 1].
get_i() - tempP[jj + 1].
get_i();
388 double distancej = tempP[ii - 1].
get_j() - tempP[jj + 1].
get_j();
389 double distancew = tempW[ii - 1] - tempW[jj + 1];
391 if (distance <= l_TOL)
395 alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
397 l_controlPoints[i].get_i() - (alfi * tempP[ii + t + 1].
get_i() + (1.0 - alfi) * tempP[ii - 1].get_i());
399 l_controlPoints[i].get_j() - (alfi * tempP[ii + t + 1].
get_j() + (1.0 - alfi) * tempP[ii - 1].get_j());
400 double distancew = l_weights[i] - (alfi * tempW[ii + t + 1] + (1.0 - alfi) * tempW[ii - 1]);
402 if (distance <= l_TOL)
411 l_controlPoints[i].set_i(tempP[i - off].get_i());
412 l_controlPoints[i].set_j(tempP[i - off].get_j());
413 l_weights[i] = tempW[i - off];
414 l_controlPoints[j].set_i(tempP[j - off].get_i());
415 l_controlPoints[j].set_j(tempP[j - off].get_j());
416 l_weights[j] = tempW[j - off];
429 for (
unsigned int k = l_r + 1; k <= m; k++)
430 l_knots[k - t] = l_knots[k];
431 j = (
unsigned int)fout;
433 for (
unsigned int k = 1; k < t; k++) {
439 for (
unsigned int k = i + 1; k <= n; k++) {
440 l_controlPoints[j].
set_i(l_controlPoints[k].get_i());
441 l_controlPoints[j].set_j(l_controlPoints[k].get_j());
442 l_weights[j] = l_weights[k];
445 for (
unsigned int k = 0; k < t; k++) {
446 l_knots.erase(l_knots.end() - 1);
447 l_controlPoints.erase(l_controlPoints.end() - 1);
450 for (
unsigned int k = 0; k < l_controlPoints.size(); k++)
451 l_controlPoints[k].set_ij(l_controlPoints[k].get_i() / l_weights[k], l_controlPoints[k].get_j() / l_weights[k]);
464 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
465 std::vector<double> &l_weights)
473 l_controlPoints.clear();
475 unsigned int n = (
unsigned int)l_crossingPoints.size() - 1;
476 unsigned int m = n + l_p + 1;
479 for (
unsigned int k = 1; k <= n; k++)
480 d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
483 std::vector<double> ubar;
485 for (
unsigned int k = 1; k < n; k++) {
486 ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
491 for (
unsigned int k = 0; k <= l_p; k++)
492 l_knots.push_back(0.0);
495 for (
unsigned int k = 1; k <= l_p; k++)
499 for (
unsigned int k = 1; k <= n - l_p; k++) {
500 l_knots.push_back(sum / l_p);
501 sum = sum - ubar[k - 1] + ubar[l_p + k - 1];
504 for (
unsigned int k = m - l_p; k <= m; k++)
505 l_knots.push_back(1.0);
510 for (
unsigned int i = 0; i <= n; i++) {
511 unsigned int span =
findSpan(ubar[i], l_p, l_knots);
513 for (
unsigned int k = 0; k <= l_p; k++)
514 A[i][span - l_p + k] = N[k].value;
523 for (
unsigned int k = 0; k <= n; k++) {
524 Qi[k] = l_crossingPoints[k].get_i();
525 Qj[k] = l_crossingPoints[k].get_j();
533 for (
unsigned int k = 0; k <= n; k++) {
535 l_controlPoints.push_back(pt);
536 l_weights.push_back(Pw[k]);
542 std::vector<vpImagePoint> v_crossingPoints;
543 l_crossingPoints.
front();
547 v_crossingPoints.push_back(pt);
548 l_crossingPoints.
next();
549 while (!l_crossingPoints.
outside()) {
550 s = l_crossingPoints.
value();
553 v_crossingPoints.push_back(pt);
556 l_crossingPoints.
next();
563 std::vector<vpImagePoint> v_crossingPoints;
564 for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
565 v_crossingPoints.push_back(*it);
573 std::vector<vpImagePoint> v_crossingPoints;
574 vpMeSite s = l_crossingPoints.front();
577 v_crossingPoints.push_back(pt);
578 std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
580 for (; it != l_crossingPoints.end(); ++it) {
583 v_crossingPoints.push_back(pt_tmp);
593 std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
594 std::vector<double> &l_weights)
597 l_controlPoints.clear();
599 unsigned int m = (
unsigned int)l_crossingPoints.size() - 1;
602 for (
unsigned int k = 1; k <= m; k++)
603 d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
606 std::vector<double> ubar;
608 for (
unsigned int k = 1; k < m; k++)
609 ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
613 for (
unsigned int k = 0; k <= l_p; k++)
614 l_knots.push_back(0.0);
616 d = (double)(m + 1) / (double)(l_n - l_p + 1);
618 for (
unsigned int j = 1; j <= l_n - l_p; j++) {
619 double i = floor(j * d);
620 double alpha = j * d - i;
621 l_knots.push_back((1.0 - alpha) * ubar[(
unsigned int)i - 1] + alpha * ubar[(
unsigned int)i]);
624 for (
unsigned int k = 0; k <= l_p; k++)
625 l_knots.push_back(1.0);
628 std::vector<vpImagePoint> Rk;
630 for (
unsigned int k = 1; k <= m - 1; k++) {
631 unsigned int span =
findSpan(ubar[k], l_p, l_knots);
632 if (span == l_p && span == l_n) {
634 vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i() -
635 N[l_p].value * l_crossingPoints[m].get_i(),
636 l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j() -
637 N[l_p].value * l_crossingPoints[m].get_j());
641 else if (span == l_p) {
643 vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i(),
644 l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j());
648 else if (span == l_n) {
650 vpImagePoint pt(l_crossingPoints[k].get_i() - N[l_p].value * l_crossingPoints[m].get_i(),
651 l_crossingPoints[k].get_j() - N[l_p].value * l_crossingPoints[m].get_j());
656 Rk.push_back(l_crossingPoints[k]);
662 for (
unsigned int i = 1; i <= m - 1; i++) {
663 unsigned int span =
findSpan(ubar[i], l_p, l_knots);
665 for (
unsigned int k = 0; k <= l_p; k++) {
666 if (N[k].i > 0 && N[k].i < l_n)
667 A[i - 1][N[k].i - 1] = N[k].value;
675 for (
unsigned int i = 0; i < l_n - 1; i++) {
677 for (
unsigned int k = 0; k < m - 1; k++)
678 sum = sum + A[k][i] * Rk[k].get_i();
681 for (
unsigned int k = 0; k < m - 1; k++)
682 sum = sum + A[k][i] * Rk[k].get_j();
685 for (
unsigned int k = 0; k < m - 1; k++)
699 l_controlPoints.push_back(l_crossingPoints[0]);
700 l_weights.push_back(1.0);
701 for (
unsigned int k = 0; k < l_n - 1; k++) {
703 l_controlPoints.push_back(pt);
704 l_weights.push_back(Pw[k]);
706 l_controlPoints.push_back(l_crossingPoints[m]);
707 l_weights.push_back(1.0);
713 std::vector<vpImagePoint> v_crossingPoints;
714 l_crossingPoints.
front();
715 while (!l_crossingPoints.
outside()) {
718 v_crossingPoints.push_back(pt);
719 l_crossingPoints.
next();
727 std::vector<vpImagePoint> v_crossingPoints;
728 for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
729 v_crossingPoints.push_back(*it);
736 std::vector<vpImagePoint> v_crossingPoints;
737 for (std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
739 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 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)