50 #include <visp3/core/vpDebug.h> 51 #include <visp3/core/vpMatrixException.h> 52 #include <visp3/vision/vpHomography.h> 57 #ifndef DOXYGEN_SHOULD_SKIP_THIS 58 const double eps = 1e-6;
93 void HLM(
unsigned int q_cible,
unsigned int nbpt,
double *xm,
double *ym,
double *xmi,
double *ymi,
vpMatrix &H);
95 void HLM(
unsigned int q_cible,
const std::vector<double> &xm,
const std::vector<double> &ym,
96 const std::vector<double> &xmi,
const std::vector<double> &ymi,
vpMatrix &H);
105 for (
unsigned int i = 0; i < 3; i++) {
106 for (
unsigned int j = 0; j < 3; j++) {
107 M[j][i] = p[pts_ref[i]][j];
108 Md[j][i] = pd[pts_ref[i]][j];
116 if (pts_ref[3] > 0) {
120 for (
unsigned int i = 0; i < 3; i++) {
121 for (
unsigned int j = 0; j < 3; j++) {
122 lamb_cour[i] = Mp[i][j] * p[pts_ref[3]][j];
123 lamb_des[i] = Mdp[i][j] * pd[pts_ref[3]][j];
127 for (
unsigned int i = 0; i < 3; i++) {
128 for (
unsigned int j = 0; j < 3; j++) {
129 M[i][j] = M[i][j] * lamb_cour[j];
130 Md[i][j] = Md[i][j] * lamb_des[j];
140 unsigned int cont_pts = 0;
141 for (
unsigned int k = 0; k < nb_pts; k++) {
142 if ((pts_ref[0] != k) && (pts_ref[1] != k) && (pts_ref[2] != k)) {
143 for (
unsigned int i = 0; i < 3; i++) {
144 pn[cont_pts][i] = 0.0;
145 pnd[cont_pts][i] = 0.0;
146 for (
unsigned int j = 0; j < 3; j++) {
147 pn[cont_pts][i] = pn[cont_pts][i] + Mp[i][j] * p[k][j];
148 pnd[cont_pts][i] = pnd[cont_pts][i] + Mdp[i][j] * pd[k][j];
151 cont_pts = cont_pts + 1;
186 unsigned int contZeros, vect;
194 for (
unsigned int j = 0; j < nb_pts; j++) {
198 M[3 * j][3] = -points_des[j][0] * points_cour[j][2];
199 M[3 * j][4] = -points_des[j][1] * points_cour[j][2];
200 M[3 * j][5] = -points_des[j][2] * points_cour[j][2];
201 M[3 * j][6] = points_des[j][0] * points_cour[j][1];
202 M[3 * j][7] = points_des[j][1] * points_cour[j][1];
203 M[3 * j][8] = points_des[j][2] * points_cour[j][1];
205 M[3 * j + 1][0] = points_des[j][0] * points_cour[j][2];
206 M[3 * j + 1][1] = points_des[j][1] * points_cour[j][2];
207 M[3 * j + 1][2] = points_des[j][2] * points_cour[j][2];
211 M[3 * j + 1][6] = -points_des[j][0] * points_cour[j][0];
212 M[3 * j + 1][7] = -points_des[j][1] * points_cour[j][0];
213 M[3 * j + 1][8] = -points_des[j][2] * points_cour[j][0];
215 M[3 * j + 2][0] = -points_des[j][0] * points_cour[j][1];
216 M[3 * j + 2][1] = -points_des[j][1] * points_cour[j][1];
217 M[3 * j + 2][2] = -points_des[j][2] * points_cour[j][1];
218 M[3 * j + 2][3] = points_des[j][0] * points_cour[j][0];
219 M[3 * j + 2][4] = points_des[j][1] * points_cour[j][0];
220 M[3 * j + 2][5] = points_des[j][2] * points_cour[j][0];
237 vals_inf = fabs(sv[0]);
240 if (fabs(sv[0]) < eps) {
241 contZeros = contZeros + 1;
243 for (
unsigned int j = 1; j < 9; j++) {
244 if (fabs(sv[j]) < vals_inf) {
245 vals_inf = fabs(sv[j]);
248 if (fabs(sv[j]) < eps) {
249 contZeros = contZeros + 1;
261 for (
unsigned int i = 0; i < 3; i++) {
262 for (
unsigned int j = 0; j < 3; j++) {
263 H[i][j] = V[3 * i + j][vect];
293 unsigned int pts_ref[4];
321 changeFrame(pts_ref, nb_pts, pd, p, pnd, pn, M, Mdp);
323 unsigned int cont_pts = nb_pts - 3;
337 for (
unsigned int i = 0; i < nc; i++)
338 for (
unsigned int j = 0; j < nc; j++)
354 unsigned int cont = 0;
355 for (
unsigned int i = 0; i < nb_pts - 5; i++) {
356 for (
unsigned int j = i + 1; j < nb_pts - 4; j++) {
357 for (
unsigned int k = j + 1; k < nb_pts - 3; k++) {
359 C[0] = pn[i][2] * pn[j][2] * pn[k][1] * pnd[k][0]
360 * (pnd[j][0] * pnd[i][1] - pnd[j][1] * pnd[i][0])
361 + pn[i][2] * pn[k][2] * pn[j][1] * pnd[j][0]
362 * (pnd[i][0] * pnd[k][1] - pnd[i][1] * pnd[k][0])
363 + pn[j][2] * pn[k][2] * pn[i][1] * pnd[i][0]
364 * (pnd[k][0] * pnd[j][1] - pnd[k][1] * pnd[j][0]);
366 C[1] = pn[i][2] * pn[j][2] * pn[k][0] * pnd[k][1]
367 * (pnd[i][0] * pnd[j][1] - pnd[i][1] * pnd[j][0])
368 + pn[i][2] * pn[k][2] * pn[j][0] * pnd[j][1]
369 * (pnd[k][0] * pnd[i][1] - pnd[k][1] * pnd[i][0])
370 + pn[j][2] * pn[k][2] * pn[i][0] * pnd[i][1]
371 * (pnd[j][0] * pnd[k][1] - pnd[j][1] * pnd[k][0]);
373 C[2] = +pn[i][1] * pn[k][1] * pn[j][2] * pnd[j][0]
374 * (pnd[k][2] * pnd[i][0] - pnd[k][0] * pnd[i][2])
375 + pn[i][1] * pn[j][1] * pn[k][2] * pnd[k][0]
376 * (pnd[i][2] * pnd[j][0] - pnd[i][0] * pnd[j][2]) +
377 pn[j][1] * pn[k][1] * pn[i][2] * pnd[i][0]
378 * (pnd[j][2] * pnd[k][0] - pnd[j][0] * pnd[k][2]);
381 C[3] = pn[i][0] * pn[j][0] * pn[k][2] * pnd[k][1]
382 * (pnd[i][2] * pnd[j][1] - pnd[i][1] * pnd[j][2])
383 + pn[i][0] * pn[k][0] * pn[j][2] * pnd[j][1]
384 * (pnd[k][2] * pnd[i][1] - pnd[k][1] * pnd[i][2])
385 + pn[j][0] * pn[k][0] * pn[i][2] * pnd[i][1]
386 * (pnd[j][2] * pnd[k][1] - pnd[j][1] * pnd[k][2]);
389 C[5] = pn[i][1] * pn[j][1] * pn[k][0] * pnd[k][2]
390 * (pnd[i][0] * pnd[j][2] - pnd[i][2] * pnd[j][0])
391 + pn[i][1] * pn[k][1] * pn[j][0] * pnd[j][2]
392 * (pnd[k][0] * pnd[i][2] - pnd[k][2] * pnd[i][0])
393 + pn[j][1] * pn[k][1] * pn[i][0] * pnd[i][2]
394 * (pnd[j][0] * pnd[k][2] - pnd[j][2] * pnd[k][0]);
396 C[6] = pn[i][0] * pn[j][0] * pn[k][1] * pnd[k][2]
397 * (pnd[i][1] * pnd[j][2] - pnd[i][2] * pnd[j][1])
398 + pn[i][0] * pn[k][0] * pn[j][1] * pnd[j][2]
399 * (pnd[k][1] * pnd[i][2] - pnd[k][2] * pnd[i][1])
400 + pn[j][0] * pn[k][0] * pn[i][1] * pnd[i][2]
401 * (pnd[j][1] * pnd[k][2] - pnd[j][2] * pnd[k][1]);
403 C[4] = pn[i][0] * pn[k][1] * pn[j][2]
404 * (pnd[k][0] * pnd[j][1] * pnd[i][2] - pnd[j][0] * pnd[i][1] * pnd[k][2])
405 + pn[k][0] * pn[i][1] * pn[j][2]
406 * (pnd[j][0] * pnd[k][1] * pnd[i][2] - pnd[i][0] * pnd[j][1] * pnd[k][2])
407 + pn[i][0] * pn[j][1] * pn[k][2]
408 * (pnd[k][0] * pnd[i][1] * pnd[j][2] - pnd[j][0] * pnd[k][1] * pnd[i][2])
409 + pn[j][0] * pn[i][1] * pn[k][2]
410 * (pnd[i][0] * pnd[k][1] * pnd[j][2] - pnd[k][0] * pnd[j][1] * pnd[i][2])
411 + pn[k][0] * pn[j][1] * pn[i][2]
412 * (pnd[j][0] * pnd[i][1] * pnd[k][2] - pnd[i][0] * pnd[k][1] * pnd[j][2])
413 + pn[j][0] * pn[k][1] * pn[i][2]
414 * (pnd[i][0] * pnd[j][1] * pnd[k][2] - pnd[k][0] * pnd[i][1] * pnd[j][2]);
418 for (
unsigned int ii = 0; ii < nc; ii++) {
419 for (
unsigned int jj = ii; jj < nc; jj++) {
420 CtC[ii][jj] = CtC[ii][jj] + C[ii] * C[jj];
428 for (
unsigned int i = 0; i < nc; i++) {
429 for (
unsigned int j = i + 1; j < nc; j++)
430 CtC[j][i] = CtC[i][j];
464 unsigned int vect = 0;
467 for (
unsigned int j = 0; j < nc; j++) {
469 if (std::fabs(sv[j] - svSorted[cont]) <= std::fabs(
vpMath::maximum(sv[j], svSorted[cont])))
471 if (std::fabs(sv[j] / svSorted[nc - 1]) < eps)
472 cont_zeros = cont_zeros + 1;
475 if (cont_zeros > 5) {
477 HLM2D(nb_pts, pd, p, H);
499 c[0][0] = V[5][vect];
501 c[1][0] = V[6][vect];
503 c[2][0] = V[3][vect];
505 c[3][0] = V[4][vect];
508 c[4][1] = V[6][vect];
510 c[5][1] = V[5][vect];
512 c[6][1] = V[2][vect];
514 c[7][1] = V[4][vect];
517 cp = c.pseudoInverse(1e-6);
529 T[0][0] = -V[1][vect];
530 T[0][1] = V[0][vect];
531 T[1][0] = V[4][vect];
532 T[1][2] = -V[2][vect];
533 T[2][0] = -V[6][vect];
534 T[2][1] = V[2][vect];
535 T[3][0] = V[6][vect];
536 T[3][2] = -V[0][vect];
537 T[4][0] = -V[3][vect];
538 T[4][1] = V[6][vect];
539 T[5][0] = V[3][vect];
540 T[5][2] = -V[1][vect];
541 T[6][0] = -V[5][vect];
542 T[6][1] = V[4][vect];
543 T[7][0] = V[5][vect];
544 T[7][2] = -V[6][vect];
545 T[8][1] = -V[5][vect];
546 T[8][2] = V[2][vect];
549 for (
unsigned int i = 0; i < 3; i++)
557 void HLM(
unsigned int q_cible,
const std::vector<double> &xm,
const std::vector<double> &ym,
558 const std::vector<double> &xmi,
const std::vector<double> &ymi,
vpMatrix &H)
560 unsigned int nbpt = (
unsigned int)xm.size();
569 for (
unsigned int i = 0; i < nbpt; i++) {
587 HLM2D(nbpt, pd, p, H);
592 HLM3D(nbpt, pd, p, H);
598 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS 622 void vpHomography::HLM(
const std::vector<double> &xb,
const std::vector<double> &yb,
const std::vector<double> &xa,
623 const std::vector<double> &ya,
bool isplanar,
vpHomography &aHb)
625 unsigned int n = (
unsigned int)xb.size();
626 if (yb.size() != n || xa.size() != n || ya.size() != n)
634 unsigned int q_cible;
642 ::HLM(q_cible, xa, ya, xb, yb, H);
void svd(vpColVector &w, vpMatrix &V)
Implementation of a matrix and operations on matrices.
static vpColVector invSort(const vpColVector &v)
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
error that can be emited by ViSP classes.
static void HLM(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, bool isplanar, vpHomography &aHb)
static Type maximum(const Type &a, const Type &b)
Implementation of an homography and operations on homographies.
Implementation of column vector and the associated operations.
error that can be emited by the vpMatrix class and its derivates
vpMatrix pseudoInverse(double svThreshold=1e-6) const