Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpHomography.cpp
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 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
51vpHomography::vpHomography() : vpArray2D<double>(3, 3), aMb(), bP() { eye(); }
52
53vpHomography::vpHomography(const vpHomography &H) : vpArray2D<double>(3, 3), aMb(), bP() { *this = H; }
54
55vpHomography::vpHomography(const vpHomogeneousMatrix &aMb, const vpPlane &bP) : vpArray2D<double>(3, 3), aMb(), bP()
56{
57 buildFrom(aMb, bP);
58}
59
61 : vpArray2D<double>(3, 3), aMb(), bP()
62{
63 buildFrom(tu, atb, p);
64}
65
67 : vpArray2D<double>(3, 3), aMb(), bP()
68{
69 buildFrom(aRb, atb, p);
70}
71
72vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : vpArray2D<double>(3, 3), aMb(), bP()
73{
74 buildFrom(arb, p);
75}
76
78{
79 insert(aMb);
80 insert(bP);
81 build();
82}
83
85{
86 insert(tu);
87 insert(atb);
88 insert(p);
89 build();
90}
91
93{
94 insert(aRb);
95 insert(atb);
96 insert(p);
97 build();
98}
99
101{
102 aMb.buildFrom(arb[0], arb[1], arb[2], arb[3], arb[4], arb[5]);
103 insert(p);
104 build();
105}
106
107/*********************************************************************/
108
109void vpHomography::insert(const vpRotationMatrix &aRb) { aMb.insert(aRb); }
110
111void vpHomography::insert(const vpHomogeneousMatrix &M) { this->aMb = M; }
112
113void vpHomography::insert(const vpThetaUVector &tu)
114{
115 vpRotationMatrix aRb(tu);
116 aMb.insert(aRb);
117}
118
119void vpHomography::insert(const vpTranslationVector &atb) { aMb.insert(atb); }
120
121void vpHomography::insert(const vpPlane &p) { this->bP = p; }
122
123vpHomography vpHomography::inverse(double sv_threshold, unsigned int *rank) const
124{
125 vpMatrix M = (*this).convert();
126 vpMatrix Minv;
127 unsigned int r = M.pseudoInverse(Minv, sv_threshold);
128 if (rank != NULL) {
129 *rank = r;
130 }
131
132 vpHomography H;
133
134 for (unsigned int i = 0; i < 3; i++)
135 for (unsigned int j = 0; j < 3; j++)
136 H[i][j] = Minv[i][j];
137
138 return H;
139}
140
141void vpHomography::inverse(vpHomography &bHa) const { bHa = inverse(); }
142
143void vpHomography::save(std::ofstream &f) const
144{
145 if (!f.fail()) {
146 f << *this;
147 } else {
148 throw(vpException(vpException::ioError, "Cannot write the homography to the output stream"));
149 }
150}
151
153{
154 vpHomography Hp;
155 for (unsigned int i = 0; i < 3; i++) {
156 for (unsigned int j = 0; j < 3; j++) {
157 double s = 0.;
158 for (unsigned int k = 0; k < 3; k++) {
159 s += (*this)[i][k] * H[k][j];
160 }
161 Hp[i][j] = s;
162 }
163 }
164 return Hp;
165}
166
168{
169 if (b.size() != 3)
170 throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d",
171 b.size()));
172
173 vpColVector a(3);
174 for (unsigned int i = 0; i < 3; i++) {
175 a[i] = 0.;
176 for (unsigned int j = 0; j < 3; j++)
177 a[i] += (*this)[i][j] * b[j];
178 }
179
180 return a;
181}
182
184{
185 vpHomography H;
186
187 for (unsigned int i = 0; i < 9; i++) {
188 H.data[i] = this->data[i] * v;
189 }
190
191 return H;
192}
193
195{
196 vpPoint a_P;
197 vpColVector v(3), v1(3);
198
199 v[0] = b_P.get_x();
200 v[1] = b_P.get_y();
201 v[2] = b_P.get_w();
202
203 v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2];
204 v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2];
205 v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2];
206
207 // v1 = M*v ;
208 a_P.set_x(v1[0]);
209 a_P.set_y(v1[1]);
210 a_P.set_w(v1[2]);
211
212 return a_P;
213}
214
216{
217 vpHomography H;
218 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
219 throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
220
221 double vinv = 1 / v;
222
223 for (unsigned int i = 0; i < 9; i++) {
224 H.data[i] = this->data[i] * vinv;
225 }
226
227 return H;
228}
229
231{
232 // if (x == 0)
233 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
234 throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
235
236 double vinv = 1 / v;
237
238 for (unsigned int i = 0; i < 9; i++)
239 data[i] *= vinv;
240
241 return *this;
242}
243
245{
246 for (unsigned int i = 0; i < 3; i++)
247 for (unsigned int j = 0; j < 3; j++)
248 (*this)[i][j] = H[i][j];
249
250 aMb = H.aMb;
251 bP = H.bP;
252 return *this;
253}
254
256{
257 if (H.getRows() != 3 || H.getCols() != 3)
258 throw(vpException(vpException::dimensionError, "The matrix is not an homography"));
259
260 for (unsigned int i = 0; i < 3; i++)
261 for (unsigned int j = 0; j < 3; j++)
262 (*this)[i][j] = H[i][j];
263
264 return *this;
265}
266
267void vpHomography::load(std::ifstream &f)
268{
269 if (!f.fail()) {
270 for (unsigned int i = 0; i < 3; i++)
271 for (unsigned int j = 0; j < 3; j++) {
272 f >> (*this)[i][j];
273 }
274 } else {
275 throw(vpException(vpException::ioError, "Cannot read the homography from the input stream"));
276 }
277}
278
286void vpHomography::build()
287{
288 vpColVector n(3);
289 vpColVector atb(3);
290 vpMatrix aRb(3, 3);
291 for (unsigned int i = 0; i < 3; i++) {
292 atb[i] = aMb[i][3];
293 for (unsigned int j = 0; j < 3; j++)
294 aRb[i][j] = aMb[i][j];
295 }
296
297 bP.getNormal(n);
298
299 double d = bP.getD();
300 vpMatrix aHb = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the
301 // plane equation. So if the plane is described by
302 // Ax+By+Cz+D=0, d=-D
303
304 for (unsigned int i = 0; i < 3; i++)
305 for (unsigned int j = 0; j < 3; j++)
306 (*this)[i][j] = aHb[i][j];
307}
308
317void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, const vpPlane &bP)
318{
319 vpColVector n(3);
320 vpColVector atb(3);
321 vpMatrix aRb(3, 3);
322 for (unsigned int i = 0; i < 3; i++) {
323 atb[i] = aMb[i][3];
324 for (unsigned int j = 0; j < 3; j++)
325 aRb[i][j] = aMb[i][j];
326 }
327
328 bP.getNormal(n);
329
330 double d = bP.getD();
331 vpMatrix aHb_ = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the
332 // plane equation. So if the plane is described by
333 // Ax+By+Cz+D=0, d=-D
334
335 for (unsigned int i = 0; i < 3; i++)
336 for (unsigned int j = 0; j < 3; j++)
337 aHb[i][j] = aHb_[i][j];
338}
339
340double vpHomography::det() const
341{
342 return ((*this)[0][0] * ((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1]) -
343 (*this)[0][1] * ((*this)[1][0] * (*this)[2][2] - (*this)[1][2] * (*this)[2][0]) +
344 (*this)[0][2] * ((*this)[1][0] * (*this)[2][1] - (*this)[1][1] * (*this)[2][0]));
345}
346
348{
349 for (unsigned int i = 0; i < 3; i++)
350 for (unsigned int j = 0; j < 3; j++)
351 if (i == j)
352 (*this)[i][j] = 1.0;
353 else
354 (*this)[i][j] = 0.0;
355}
356
357#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
365#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
366
368{
369 double xa = iPa.get_u();
370 double ya = iPa.get_v();
372 double z = xa * bGa[2][0] + ya * bGa[2][1] + bGa[2][2];
373 double xb = (xa * bGa[0][0] + ya * bGa[0][1] + bGa[0][2]) / z;
374 double yb = (xa * bGa[1][0] + ya * bGa[1][1] + bGa[1][2]) / z;
375
376 vpImagePoint iPb(yb, xb);
377
378 return iPb;
379}
380
382{
383 double xa = Pa.get_x();
384 double ya = Pa.get_y();
385 double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2];
386 double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z;
387 double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z;
388
389 vpPoint Pb;
390 Pb.set_x(xb);
391 Pb.set_y(yb);
392
393 return Pb;
394}
395
396
397void vpHomography::robust(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
398 const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inliers,
399 double &residual, double weights_threshold, unsigned int niter, bool normalization)
400{
401 unsigned int n = (unsigned int)xb.size();
402 if (yb.size() != n || xa.size() != n || ya.size() != n)
403 throw(vpException(vpException::dimensionError, "Bad dimension for robust homography estimation"));
404
405 // 4 point are required
406 if (n < 4)
407 throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
408
409 try {
410 std::vector<double> xan, yan, xbn, ybn;
411
412 double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
413
414 vpHomography aHbn;
415
416 if (normalization) {
417 vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
418 vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
419 } else {
420 xbn = xb;
421 ybn = yb;
422 xan = xa;
423 yan = ya;
424 }
425
426 unsigned int nbLinesA = 2;
427 vpMatrix A(nbLinesA * n, 8);
428 vpColVector X(8);
429 vpColVector Y(nbLinesA * n);
430 vpMatrix W(nbLinesA * n, nbLinesA * n, 0); // Weight matrix
431
432 vpColVector w(nbLinesA * n);
433
434 // All the weights are set to 1 at the beginning to use a classical least
435 // square scheme
436 w = 1;
437 // Update the square matrix associated to the weights
438 for (unsigned int i = 0; i < nbLinesA * n; i++) {
439 W[i][i] = w[i];
440 }
441
442 // build matrix A
443 for (unsigned int i = 0; i < n; i++) {
444 A[nbLinesA * i][0] = xbn[i];
445 A[nbLinesA * i][1] = ybn[i];
446 A[nbLinesA * i][2] = 1;
447 A[nbLinesA * i][3] = 0;
448 A[nbLinesA * i][4] = 0;
449 A[nbLinesA * i][5] = 0;
450 A[nbLinesA * i][6] = -xbn[i] * xan[i];
451 A[nbLinesA * i][7] = -ybn[i] * xan[i];
452
453 A[nbLinesA * i + 1][0] = 0;
454 A[nbLinesA * i + 1][1] = 0;
455 A[nbLinesA * i + 1][2] = 0;
456 A[nbLinesA * i + 1][3] = xbn[i];
457 A[nbLinesA * i + 1][4] = ybn[i];
458 A[nbLinesA * i + 1][5] = 1;
459 A[nbLinesA * i + 1][6] = -xbn[i] * yan[i];
460 A[nbLinesA * i + 1][7] = -ybn[i] * yan[i];
461
462 Y[nbLinesA * i] = xan[i];
463 Y[nbLinesA * i + 1] = yan[i];
464 }
465
466 vpMatrix WA;
467 vpMatrix WAp;
468 unsigned int iter = 0;
469 vpRobust r; // M-Estimator
470
471 while (iter < niter) {
472 WA = W * A;
473
474 X = WA.pseudoInverse(1e-26) * W * Y;
475 vpColVector residu;
476 residu = Y - A * X;
477
478 // Compute the weights using the Tukey biweight M-Estimator
479 r.MEstimator(vpRobust::TUKEY, residu, w);
480
481 // Update the weights matrix
482 for (unsigned int i = 0; i < n * nbLinesA; i++) {
483 W[i][i] = w[i];
484 }
485 // Build the homography
486 for (unsigned int i = 0; i < 8; i++)
487 aHbn.data[i] = X[i];
488 aHbn[2][2] = 1;
489
490 iter++;
491 }
492 inliers.resize(n);
493 unsigned int nbinliers = 0;
494 for (unsigned int i = 0; i < n; i++) {
495 if (w[i * 2] < weights_threshold && w[i * 2 + 1] < weights_threshold)
496 inliers[i] = false;
497 else {
498 inliers[i] = true;
499 nbinliers++;
500 }
501 }
502
503 if (normalization) {
504 // H after denormalization
505 vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
506 } else {
507 aHb = aHbn;
508 }
509
510 residual = 0;
511 vpColVector a(3), b(3), c(3);
512 for (unsigned int i = 0; i < n; i++) {
513 if (inliers[i]) {
514 a[0] = xa[i];
515 a[1] = ya[i];
516 a[2] = 1;
517 b[0] = xb[i];
518 b[1] = yb[i];
519 b[2] = 1;
520
521 c = aHb * b;
522 c /= c[2];
523 residual += (a - c).sumSquare();
524 }
525 }
526
527 residual = sqrt(residual / nbinliers);
528 } catch (...) {
529 throw(vpException(vpException::fatalError, "Cannot estimate an homography"));
530 }
531}
532
534{
535 vpImagePoint ipa;
536 double u = ipb.get_u();
537 double v = ipb.get_v();
538
539 double u_a = (*this)[0][0] * u + (*this)[0][1] * v + (*this)[0][2];
540 double v_a = (*this)[1][0] * u + (*this)[1][1] * v + (*this)[1][2];
541 double w_a = (*this)[2][0] * u + (*this)[2][1] * v + (*this)[2][2];
542
543 if (std::fabs(w_a) > std::numeric_limits<double>::epsilon()) {
544 ipa.set_u(u_a / w_a);
545 ipa.set_v(v_a / w_a);
546 }
547
548 return ipa;
549}
550
552{
553 vpMatrix M(3, 3);
554 for (unsigned int i = 0; i < 3; i++)
555 for (unsigned int j = 0; j < 3; j++)
556 M[i][j] = (*this)[i][j];
557
558 return M;
559}
560
562{
563 vpHomography H;
564 double px = cam.get_px();
565 double py = cam.get_py();
566 double u0 = cam.get_u0();
567 double v0 = cam.get_v0();
568 double one_over_px = cam.get_px_inverse();
569 double one_over_py = cam.get_py_inverse();
570 double h00 = (*this)[0][0], h01 = (*this)[0][1], h02 = (*this)[0][2];
571 double h10 = (*this)[1][0], h11 = (*this)[1][1], h12 = (*this)[1][2];
572 double h20 = (*this)[2][0], h21 = (*this)[2][1], h22 = (*this)[2][2];
573
574 double u0_one_over_px = u0 * one_over_px;
575 double v0_one_over_py = v0 * one_over_py;
576
577 double A = h00 * one_over_px - h20 * u0_one_over_px;
578 double B = h01 * one_over_px - h21 * u0_one_over_px;
579 double C = h02 * one_over_px - h22 * u0_one_over_px;
580 double D = h10 * one_over_py - h20 * v0_one_over_py;
581 double E = h11 * one_over_py - h21 * v0_one_over_py;
582 double F = h12 * one_over_py - h22 * v0_one_over_py;
583
584 H[0][0] = A * px;
585 H[1][0] = D * px;
586 H[2][0] = h20 * px;
587
588 H[0][1] = B * py;
589 H[1][1] = E * py;
590 H[2][1] = h21 * py;
591
592 H[0][2] = A * u0 + B * v0 + C;
593 H[1][2] = D * u0 + E * v0 + F;
594 H[2][2] = h20 * u0 + h21 * v0 + h22;
595
596 return H;
597}
598
600{
601 vpHomography H;
602 double px = cam.get_px();
603 double py = cam.get_py();
604 double u0 = cam.get_u0();
605 double v0 = cam.get_v0();
606 double one_over_px = cam.get_px_inverse();
607 double one_over_py = cam.get_py_inverse();
608 double h00 = (*this)[0][0], h01 = (*this)[0][1], h02 = (*this)[0][2];
609 double h10 = (*this)[1][0], h11 = (*this)[1][1], h12 = (*this)[1][2];
610 double h20 = (*this)[2][0], h21 = (*this)[2][1], h22 = (*this)[2][2];
611
612 double A = h00 * px + u0 * h20;
613 double B = h01 * px + u0 * h21;
614 double C = h02 * px + u0 * h22;
615 double D = h10 * py + v0 * h20;
616 double E = h11 * py + v0 * h21;
617 double F = h12 * py + v0 * h22;
618
619 H[0][0] = A * one_over_px;
620 H[1][0] = D * one_over_px;
621 H[2][0] = h20 * one_over_px;
622
623 H[0][1] = B * one_over_py;
624 H[1][1] = E * one_over_py;
625 H[2][1] = h21 * one_over_py;
626
627 double u0_one_over_px = u0 * one_over_px;
628 double v0_one_over_py = v0 * one_over_py;
629
630 H[0][2] = -A * u0_one_over_px - B * v0_one_over_py + C;
631 H[1][2] = -D * u0_one_over_px - E * v0_one_over_py + F;
632 H[2][2] = -h20 * u0_one_over_px - h21 * v0_one_over_py + h22;
633
634 return H;
635}
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition vpArray2D.h:131
unsigned int getCols() const
Definition vpArray2D.h:280
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
unsigned int getRows() const
Definition vpArray2D.h:290
Generic class defining intrinsic camera parameters.
double get_px_inverse() const
double get_py_inverse() const
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
Definition vpException.h:59
@ ioError
I/O error.
Definition vpException.h:79
@ dimensionError
Bad dimension.
Definition vpException.h:83
@ fatalError
Fatal error.
Definition vpException.h:84
@ divideByZeroError
Division by zero.
Definition vpException.h:82
Implementation of an homogeneous matrix and operations on such kind of matrices.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void insert(const vpRotationMatrix &R)
Implementation of an homography and operations on homographies.
vpHomography collineation2homography(const vpCameraParameters &cam) const
vpHomography inverse(double sv_threshold=1e-16, unsigned int *rank=NULL) 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 & 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)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double get_u() const
void set_u(double u)
void set_v(double v)
double get_v() const
error that can be emitted by the vpMatrix class and its derivatives
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
vpMatrix pseudoInverse(double svThreshold=1e-6) const
This class defines the container for a plane geometrical structure.
Definition vpPlane.h:54
double getD() const
Definition vpPlane.h:106
vpColVector getNormal() const
Definition vpPlane.cpp:245
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
double get_w() const
Get the point w coordinate in the image plane.
Definition vpPoint.cpp:471
void set_x(double x)
Set the point x coordinate in the image plane.
Definition vpPoint.cpp:508
double get_y() const
Get the point y coordinate in the image plane.
Definition vpPoint.cpp:469
double get_x() const
Get the point x coordinate in the image plane.
Definition vpPoint.cpp:467
void set_y(double y)
Set the point y coordinate in the image plane.
Definition vpPoint.cpp:510
void set_w(double w)
Set the point w coordinate in the image plane.
Definition vpPoint.cpp:512
Implementation of a pose vector and operations on poses.
Contains an M-estimator and various influence function.
Definition vpRobust.h:83
@ TUKEY
Tukey influence function.
Definition vpRobust.h:87
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
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.