Visual Servoing Platform  version 3.6.1 under development (2024-07-23)
vpHomography.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Homography transformation.
32  */
33 
40 #include <stdio.h>
41 
42 #include <visp3/core/vpDebug.h>
43 #include <visp3/core/vpMatrix.h>
44 #include <visp3/core/vpRobust.h>
45 #include <visp3/vision/vpHomography.h>
46 
47 // Exception
48 #include <visp3/core/vpException.h>
49 #include <visp3/core/vpMatrixException.h>
50 
52 
53 vpHomography::vpHomography() : vpArray2D<double>(3, 3), m_aMb(), m_bP() { eye(); }
54 
55 vpHomography::vpHomography(const vpHomography &H) : vpArray2D<double>(3, 3), m_aMb(), m_bP() { *this = H; }
56 
57 vpHomography::vpHomography(const vpHomogeneousMatrix &aMb, const vpPlane &bP) : vpArray2D<double>(3, 3), m_aMb(), m_bP()
58 {
59  build(aMb, bP);
60 }
61 
63  : vpArray2D<double>(3, 3), m_aMb(), m_bP()
64 {
65  build(tu, atb, p);
66 }
67 
69  : vpArray2D<double>(3, 3), m_aMb(), m_bP()
70 {
71  build(aRb, atb, p);
72 }
73 
74 vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : vpArray2D<double>(3, 3), m_aMb(), m_bP()
75 {
76  build(arb, p);
77 }
78 
79 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
84 {
85  build(aMb, bP);
86 }
87 
92 {
93  build(tu, atb, p);
94 }
95 
100 {
101  build(aRb, atb, p);
102 }
103 
107 void vpHomography::buildFrom(const vpPoseVector &arb, const vpPlane &p)
108 {
109  build(arb, p);
110 }
111 #endif
112 
114 {
115  insert(aMb);
116  insert(bP);
117  build();
118  return *this;
119 }
120 
122 {
123  insert(tu);
124  insert(atb);
125  insert(p);
126  build();
127  return *this;
128 }
129 
131 {
132  insert(aRb);
133  insert(atb);
134  insert(p);
135  build();
136  return *this;
137 }
138 
140 {
141  double tx = arb[0], ty = arb[1], tz = arb[2], tux = arb[3], tuy = arb[4], tuz = arb[5];
142  m_aMb.build(tx, ty, tz, tux, tuy, tuz);
143  insert(p);
144  build();
145  return *this;
146 }
147 
148 /*********************************************************************/
149 
150 void vpHomography::insert(const vpRotationMatrix &aRb) { m_aMb.insert(aRb); }
151 
152 void vpHomography::insert(const vpHomogeneousMatrix &M) { m_aMb = M; }
153 
154 void vpHomography::insert(const vpThetaUVector &tu)
155 {
156  vpRotationMatrix aRb(tu);
157  m_aMb.insert(aRb);
158 }
159 
160 void vpHomography::insert(const vpTranslationVector &atb) { m_aMb.insert(atb); }
161 
162 void vpHomography::insert(const vpPlane &bP) { m_bP = bP; }
163 
164 vpHomography vpHomography::inverse(double sv_threshold, unsigned int *rank) const
165 {
166  vpMatrix M = (*this).convert();
167  vpMatrix Minv;
168  unsigned int r = M.pseudoInverse(Minv, sv_threshold);
169  if (rank != nullptr) {
170  *rank = r;
171  }
172 
173  vpHomography H;
174  unsigned int nbRows = H.getRows();
175  unsigned int nbCols = H.getCols();
176  for (unsigned int i = 0; i < nbRows; ++i) {
177  for (unsigned int j = 0; j < nbCols; ++j) {
178  H[i][j] = Minv[i][j];
179  }
180  }
181  return H;
182 }
183 
184 void vpHomography::inverse(vpHomography &bHa) const { bHa = inverse(); }
185 
186 void vpHomography::save(std::ofstream &f) const
187 {
188  if (!f.fail()) {
189  f << *this;
190  }
191  else {
192  throw(vpException(vpException::ioError, "Cannot write the homography to the output stream"));
193  }
194 }
195 
197 {
198  vpHomography Hp;
199  const unsigned int nbCols = 3, nbRows = 3;
200  for (unsigned int i = 0; i < nbRows; ++i) {
201  for (unsigned int j = 0; j < nbCols; ++j) {
202  double s = 0.;
203  for (unsigned int k = 0; k < nbCols; ++k) {
204  s += (*this)[i][k] * H[k][j];
205  }
206  Hp[i][j] = s;
207  }
208  }
209  return Hp;
210 }
211 
213 {
214  const unsigned int requiredSize = 3;
215  if (b.size() != requiredSize) {
216  throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d",
217  b.size()));
218  }
219 
220  vpColVector a(requiredSize);
221  for (unsigned int i = 0; i < requiredSize; ++i) {
222  a[i] = 0.;
223  for (unsigned int j = 0; j < requiredSize; ++j) {
224  a[i] += (*this)[i][j] * b[j];
225  }
226  }
227 
228  return a;
229 }
230 
231 vpHomography vpHomography::operator*(const double &v) const
232 {
233  const unsigned int nbData = 9; // cols x rows = 3 x 3
234  vpHomography H;
235 
236  for (unsigned int i = 0; i < nbData; ++i) {
237  H.data[i] = this->data[i] * v;
238  }
239 
240  return H;
241 }
242 
244 {
245  vpPoint a_P;
246  vpColVector v(3), v1(3);
247 
248  v[0] = b_P.get_x();
249  v[1] = b_P.get_y();
250  v[2] = b_P.get_w();
251 
252  v1[0] = ((*this)[0][0] * v[0]) + ((*this)[0][1] * v[1]) + ((*this)[0][2] * v[2]);
253  v1[1] = ((*this)[1][0] * v[0]) + ((*this)[1][1] * v[1]) + ((*this)[1][2] * v[2]);
254  v1[2] = ((*this)[2][0] * v[0]) + ((*this)[2][1] * v[1]) + ((*this)[2][2] * v[2]);
255 
256  // v1 is equal to M v ;
257  a_P.set_x(v1[0]);
258  a_P.set_y(v1[1]);
259  a_P.set_w(v1[2]);
260 
261  return a_P;
262 }
263 
264 vpHomography vpHomography::operator/(const double &v) const
265 {
266  vpHomography H;
267  if (std::fabs(v) <= std::numeric_limits<double>::epsilon()) {
268  throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
269  }
270 
271  double vinv = 1 / v;
272 
273  const unsigned int nbData = 9; // cols x rows = 3 x 3
274  for (unsigned int i = 0; i < nbData; ++i) {
275  H.data[i] = this->data[i] * vinv;
276  }
277 
278  return H;
279 }
280 
282 {
283  if (std::fabs(v) <= std::numeric_limits<double>::epsilon()) {
284  throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
285  }
286 
287  double vinv = 1 / v;
288 
289  const unsigned int nbData = 9; // cols x rows = 3 x 3
290  for (unsigned int i = 0; i < nbData; ++i) {
291  data[i] *= vinv;
292  }
293 
294  return *this;
295 }
296 
298 {
299  const unsigned int nbCols = 3, nbRows = 3;
300  for (unsigned int i = 0; i < nbRows; ++i) {
301  for (unsigned int j = 0; j < nbCols; ++j) {
302  (*this)[i][j] = H[i][j];
303  }
304  }
305 
306  m_aMb = H.m_aMb;
307  m_bP = H.m_bP;
308  return *this;
309 }
310 
312 {
313  const unsigned int nbCols = 3, nbRows = 3;
314  if ((H.getRows() != nbRows) || (H.getCols() != nbCols)) {
315  throw(vpException(vpException::dimensionError, "The matrix is not an homography"));
316  }
317 
318  for (unsigned int i = 0; i < nbRows; ++i) {
319  for (unsigned int j = 0; j < nbCols; ++j) {
320  (*this)[i][j] = H[i][j];
321  }
322  }
323 
324  return *this;
325 }
326 
327 void vpHomography::load(std::ifstream &f)
328 {
329  const unsigned int nbCols = 3, nbRows = 3;
330  if (!f.fail()) {
331  for (unsigned int i = 0; i < nbRows; ++i) {
332  for (unsigned int j = 0; j < nbCols; ++j) {
333  f >> (*this)[i][j];
334  }
335  }
336  }
337  else {
338  throw(vpException(vpException::ioError, "Cannot read the homography from the input stream"));
339  }
340 }
341 
349 void vpHomography::build()
350 {
351  const unsigned int nbCols = 3, nbRows = 3;
352  vpColVector n(nbRows);
353  vpColVector atb(nbRows);
354  vpMatrix aRb(nbRows, nbCols);
355  for (unsigned int i = 0; i < nbRows; ++i) {
356  atb[i] = m_aMb[i][3];
357  for (unsigned int j = 0; j < nbCols; ++j) {
358  aRb[i][j] = m_aMb[i][j];
359  }
360  }
361 
362  m_bP.getNormal(n);
363 
364  double d = m_bP.getD();
365  vpMatrix aHb = aRb - ((atb * n.t()) / d); // the d used in the equation is such as nX=d is the
366  // plane equation. So if the plane is described by
367  // Ax+By+Cz+D=0, d=-D
368 
369  for (unsigned int i = 0; i < nbRows; ++i) {
370  for (unsigned int j = 0; j < nbCols; ++j) {
371  (*this)[i][j] = aHb[i][j];
372  }
373  }
374 }
375 
376 #ifndef DOXYGEN_SHOULD_SKIP_THIS
388 void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, const vpPlane &bP)
389 {
390  const unsigned int nbCols = 3, nbRows = 3;
391  vpColVector n(nbRows);
392  vpColVector atb(nbRows);
393  vpMatrix aRb(nbRows, nbCols);
394  for (unsigned int i = 0; i < nbRows; ++i) {
395  atb[i] = aMb[i][3];
396  for (unsigned int j = 0; j < nbCols; ++j) {
397  aRb[i][j] = aMb[i][j];
398  }
399  }
400 
401  bP.getNormal(n);
402 
403  double d = bP.getD();
404  vpMatrix aHb_ = aRb - ((atb * n.t()) / d); // the d used in the equation is such as nX=d is the
405  // plane equation. So if the plane is described by
406  // Ax+By+Cz+D=0, d=-D
407 
408  for (unsigned int i = 0; i < nbRows; ++i) {
409  for (unsigned int j = 0; j < nbCols; ++j) {
410  aHb[i][j] = aHb_[i][j];
411  }
412  }
413 }
414 #endif
415 
416 double vpHomography::det() const
417 {
418  return ((((*this)[0][0] * (((*this)[1][1] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][1]))) -
419  ((*this)[0][1] * (((*this)[1][0] * (*this)[2][2]) - ((*this)[1][2] * (*this)[2][0])))) +
420  ((*this)[0][2] * (((*this)[1][0] * (*this)[2][1]) - ((*this)[1][1] * (*this)[2][0]))));
421 }
422 
424 {
425  const unsigned int nbCols = 3, nbRows = 3;
426  for (unsigned int i = 0; i < nbRows; ++i) {
427  for (unsigned int j = 0; j < nbCols; ++j) {
428  if (i == j) {
429  (*this)[i][j] = 1.0;
430  }
431  else {
432  (*this)[i][j] = 0.0;
433  }
434  }
435  }
436 }
437 
439 {
440  double xa = iPa.get_u();
441  double ya = iPa.get_v();
442  vpHomography bGa = bHa.homography2collineation(cam);
443  double z = (xa * bGa[2][0]) + (ya * bGa[2][1]) + bGa[2][2];
444  double xb = ((xa * bGa[0][0]) + (ya * bGa[0][1]) + bGa[0][2]) / z;
445  double yb = ((xa * bGa[1][0]) + (ya * bGa[1][1]) + bGa[1][2]) / z;
446 
447  vpImagePoint iPb(yb, xb);
448 
449  return iPb;
450 }
451 
453 {
454  double xa = Pa.get_x();
455  double ya = Pa.get_y();
456  double z = (xa * bHa[2][0]) + (ya * bHa[2][1]) + bHa[2][2];
457  double xb = ((xa * bHa[0][0]) + (ya * bHa[0][1]) + bHa[0][2]) / z;
458  double yb = ((xa * bHa[1][0]) + (ya * bHa[1][1]) + bHa[1][2]) / z;
459 
460  vpPoint Pb;
461  Pb.set_x(xb);
462  Pb.set_y(yb);
463 
464  return Pb;
465 }
466 
467 
468 void vpHomography::robust(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
469  const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inliers,
470  double &residual, double weights_threshold, unsigned int niter, bool normalization)
471 {
472  unsigned int n = static_cast<unsigned int>(xb.size());
473  if ((yb.size() != n) || (xa.size() != n) || (ya.size() != n)) {
474  throw(vpException(vpException::dimensionError, "Bad dimension for robust homography estimation"));
475  }
476 
477  // 4 point are required
478  const unsigned int nbRequiredPoints = 4;
479  if (n < nbRequiredPoints) {
480  throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
481  }
482 
483  try {
484  std::vector<double> xan, yan, xbn, ybn;
485 
486  double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
487 
488  vpHomography aHbn;
489 
490  if (normalization) {
491  vpHomography::hartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
492  vpHomography::hartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
493  }
494  else {
495  xbn = xb;
496  ybn = yb;
497  xan = xa;
498  yan = ya;
499  }
500 
501  unsigned int nbLinesA = 2;
502  vpMatrix A(nbLinesA * n, 8);
503  vpColVector X(8);
504  vpColVector Y(nbLinesA * n);
505  vpMatrix W(nbLinesA * n, nbLinesA * n, 0); // Weight matrix
506 
507  vpColVector w(nbLinesA * n);
508 
509  // All the weights are set to 1 at the beginning to use a classical least
510  // square scheme
511  w = 1;
512  // Update the square matrix associated to the weights
513  for (unsigned int i = 0; i < (nbLinesA * n); ++i) {
514  W[i][i] = w[i];
515  }
516 
517  // build matrix A
518  for (unsigned int i = 0; i < n; ++i) {
519  A[nbLinesA * i][0] = xbn[i];
520  A[nbLinesA * i][1] = ybn[i];
521  A[nbLinesA * i][2] = 1;
522  A[nbLinesA * i][3] = 0;
523  A[nbLinesA * i][4] = 0;
524  A[nbLinesA * i][5] = 0;
525  A[nbLinesA * i][6] = -xbn[i] * xan[i];
526  A[nbLinesA * i][7] = -ybn[i] * xan[i];
527 
528  A[(nbLinesA * i) + 1][0] = 0;
529  A[(nbLinesA * i) + 1][1] = 0;
530  A[(nbLinesA * i) + 1][2] = 0;
531  A[(nbLinesA * i) + 1][3] = xbn[i];
532  A[(nbLinesA * i) + 1][4] = ybn[i];
533  A[(nbLinesA * i) + 1][5] = 1;
534  A[(nbLinesA * i) + 1][6] = -xbn[i] * yan[i];
535  A[(nbLinesA * i) + 1][7] = -ybn[i] * yan[i];
536 
537  Y[nbLinesA * i] = xan[i];
538  Y[(nbLinesA * i) + 1] = yan[i];
539  }
540 
541  vpMatrix WA;
542  vpMatrix WAp;
543  unsigned int iter = 0;
544  vpRobust r; // M-Estimator
545 
546  while (iter < niter) {
547  WA = W * A;
548 
549  X = WA.pseudoInverse(1e-26) * W * Y;
550  vpColVector residu;
551  residu = Y - (A * X);
552 
553  // Compute the weights using the Tukey biweight M-Estimator
554  r.MEstimator(vpRobust::TUKEY, residu, w);
555 
556  // Update the weights matrix
557  for (unsigned int i = 0; i < (n * nbLinesA); ++i) {
558  W[i][i] = w[i];
559  }
560  // Build the homography
561  for (unsigned int i = 0; i < 8; ++i) {
562  aHbn.data[i] = X[i];
563  }
564  aHbn[2][2] = 1;
565 
566  ++iter;
567  }
568  inliers.resize(n);
569  unsigned int nbinliers = 0;
570  for (unsigned int i = 0; i < n; ++i) {
571  if ((w[i * 2] < weights_threshold) && (w[(i * 2) + 1] < weights_threshold)) {
572  inliers[i] = false;
573  }
574  else {
575  inliers[i] = true;
576  ++nbinliers;
577  }
578  }
579 
580  if (normalization) {
581  // H after denormalization
582  vpHomography::hartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
583  }
584  else {
585  aHb = aHbn;
586  }
587 
588  residual = 0;
589  vpColVector a(3), b(3), c(3);
590  for (unsigned int i = 0; i < n; ++i) {
591  if (inliers[i]) {
592  a[0] = xa[i];
593  a[1] = ya[i];
594  a[2] = 1;
595  b[0] = xb[i];
596  b[1] = yb[i];
597  b[2] = 1;
598 
599  c = aHb * b;
600  c /= c[2];
601  residual += (a - c).sumSquare();
602  }
603  }
604 
605  residual = sqrt(residual / nbinliers);
606  }
607  catch (...) {
608  throw(vpException(vpException::fatalError, "Cannot estimate an homography"));
609  }
610 }
611 
613 {
614  vpImagePoint ipa;
615  double u = ipb.get_u();
616  double v = ipb.get_v();
617 
618  double u_a = ((*this)[0][0] * u) + ((*this)[0][1] * v) + (*this)[0][2];
619  double v_a = ((*this)[1][0] * u) + ((*this)[1][1] * v) + (*this)[1][2];
620  double w_a = ((*this)[2][0] * u) + ((*this)[2][1] * v) + (*this)[2][2];
621 
622  if (std::fabs(w_a) > std::numeric_limits<double>::epsilon()) {
623  ipa.set_u(u_a / w_a);
624  ipa.set_v(v_a / w_a);
625  }
626 
627  return ipa;
628 }
629 
631 {
632  const unsigned int nbRows = 3, nbCols = 3;
633  vpMatrix M(nbRows, nbCols);
634  for (unsigned int i = 0; i < nbRows; ++i) {
635  for (unsigned int j = 0; j < nbCols; ++j) {
636  M[i][j] = (*this)[i][j];
637  }
638  }
639 
640  return M;
641 }
642 
644 {
645  vpHomography H;
646  double px = cam.get_px();
647  double py = cam.get_py();
648  double u0 = cam.get_u0();
649  double v0 = cam.get_v0();
650  double one_over_px = cam.get_px_inverse();
651  double one_over_py = cam.get_py_inverse();
652  double h00 = (*this)[0][0], h01 = (*this)[0][1], h02 = (*this)[0][2];
653  double h10 = (*this)[1][0], h11 = (*this)[1][1], h12 = (*this)[1][2];
654  double h20 = (*this)[2][0], h21 = (*this)[2][1], h22 = (*this)[2][2];
655 
656  double u0_one_over_px = u0 * one_over_px;
657  double v0_one_over_py = v0 * one_over_py;
658 
659  double A = (h00 * one_over_px) - (h20 * u0_one_over_px);
660  double B = (h01 * one_over_px) - (h21 * u0_one_over_px);
661  double C = (h02 * one_over_px) - (h22 * u0_one_over_px);
662  double D = (h10 * one_over_py) - (h20 * v0_one_over_py);
663  double E = (h11 * one_over_py) - (h21 * v0_one_over_py);
664  double F = (h12 * one_over_py) - (h22 * v0_one_over_py);
665 
666  H[0][0] = A * px;
667  H[1][0] = D * px;
668  H[2][0] = h20 * px;
669 
670  H[0][1] = B * py;
671  H[1][1] = E * py;
672  H[2][1] = h21 * py;
673 
674  H[0][2] = (A * u0) + (B * v0) + C;
675  H[1][2] = (D * u0) + (E * v0) + F;
676  H[2][2] = (h20 * u0) + (h21 * v0) + h22;
677 
678  return H;
679 }
680 
682 {
683  vpHomography H;
684  double px = cam.get_px();
685  double py = cam.get_py();
686  double u0 = cam.get_u0();
687  double v0 = cam.get_v0();
688  double one_over_px = cam.get_px_inverse();
689  double one_over_py = cam.get_py_inverse();
690  double h00 = (*this)[0][0], h01 = (*this)[0][1], h02 = (*this)[0][2];
691  double h10 = (*this)[1][0], h11 = (*this)[1][1], h12 = (*this)[1][2];
692  double h20 = (*this)[2][0], h21 = (*this)[2][1], h22 = (*this)[2][2];
693 
694  double A = (h00 * px) + (u0 * h20);
695  double B = (h01 * px) + (u0 * h21);
696  double C = (h02 * px) + (u0 * h22);
697  double D = (h10 * py) + (v0 * h20);
698  double E = (h11 * py) + (v0 * h21);
699  double F = (h12 * py) + (v0 * h22);
700 
701  H[0][0] = A * one_over_px;
702  H[1][0] = D * one_over_px;
703  H[2][0] = h20 * one_over_px;
704 
705  H[0][1] = B * one_over_py;
706  H[1][1] = E * one_over_py;
707  H[2][1] = h21 * one_over_py;
708 
709  double u0_one_over_px = u0 * one_over_px;
710  double v0_one_over_py = v0 * one_over_py;
711 
712  H[0][2] = ((-A * u0_one_over_px) - (B * v0_one_over_py)) + C;
713  H[1][2] = ((-D * u0_one_over_px) - (E * v0_one_over_py)) + F;
714  H[2][2] = ((-h20 * u0_one_over_px) - (h21 * v0_one_over_py)) + h22;
715 
716  return H;
717 }
718 
719 END_VISP_NAMESPACE
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:145
unsigned int getCols() const
Definition: vpArray2D.h:337
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
unsigned int getRows() const
Definition: vpArray2D.h:347
Generic class defining intrinsic camera parameters.
double get_px_inverse() const
double get_py_inverse() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ ioError
I/O error.
Definition: vpException.h:67
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix & build(const vpTranslationVector &t, const vpRotationMatrix &R)
void insert(const vpRotationMatrix &R)
Implementation of an homography and operations on homographies.
Definition: vpHomography.h:174
vpHomography collineation2homography(const vpCameraParameters &cam) const
void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP)
Construction from translation and rotation and a plane.
vpImagePoint projection(const vpImagePoint &ipb)
double det() const
vpHomography inverse(double sv_threshold=1e-16, unsigned int *rank=nullptr) const
vpHomography & operator/=(double v)
void save(std::ofstream &f) const
vpHomography operator*(const vpHomography &H) const
vpHomography operator/(const double &v) const
vpHomography & operator=(const vpHomography &H)
vpHomography homography2collineation(const vpCameraParameters &cam) const
vpMatrix convert() const
static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa)
void load(std::ifstream &f)
static void robust(const std::vector< double > &xb, const std::vector< double > &yb, const std::vector< double > &xa, const std::vector< double > &ya, vpHomography &aHb, std::vector< bool > &inliers, double &residual, double weights_threshold=0.4, unsigned int niter=4, bool normalization=true)
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true)
Definition: vpHomography.h:419
vpHomography & build(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP)
Construction from translation and rotation and a plane.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
double get_u() const
Definition: vpImagePoint.h:136
void set_u(double u)
Definition: vpImagePoint.h:335
void set_v(double v)
Definition: vpImagePoint.h:346
double get_v() const
Definition: vpImagePoint.h:147
error that can be emitted by the vpMatrix class and its derivatives
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
vpMatrix pseudoInverse(double svThreshold=1e-6) const
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
double getD() const
Definition: vpPlane.h:106
vpColVector getNormal() const
Definition: vpPlane.cpp:258
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:424
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:464
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:422
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:420
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:466
void set_w(double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:468
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:203
Contains an M-estimator and various influence function.
Definition: vpRobust.h:84
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:89
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:130
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpRowVector t() const