42 #include <visp3/core/vpDebug.h>
43 #include <visp3/core/vpMatrix.h>
44 #include <visp3/core/vpRobust.h>
45 #include <visp3/vision/vpHomography.h>
48 #include <visp3/core/vpException.h>
49 #include <visp3/core/vpMatrixException.h>
63 :
vpArray2D<double>(3, 3), m_aMb(), m_bP()
69 :
vpArray2D<double>(3, 3), m_aMb(), m_bP()
79 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
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);
162 void vpHomography::insert(
const vpPlane &bP) { m_bP = bP; }
169 if (rank !=
nullptr) {
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];
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) {
203 for (
unsigned int k = 0; k < nbCols; ++k) {
204 s += (*this)[i][k] * H[k][j];
214 const unsigned int requiredSize = 3;
215 if (b.
size() != requiredSize) {
221 for (
unsigned int i = 0; i < requiredSize; ++i) {
223 for (
unsigned int j = 0; j < requiredSize; ++j) {
224 a[i] += (*this)[i][j] * b[j];
233 const unsigned int nbData = 9;
236 for (
unsigned int i = 0; i < nbData; ++i) {
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]);
267 if (std::fabs(v) <= std::numeric_limits<double>::epsilon()) {
273 const unsigned int nbData = 9;
274 for (
unsigned int i = 0; i < nbData; ++i) {
283 if (std::fabs(v) <= std::numeric_limits<double>::epsilon()) {
289 const unsigned int nbData = 9;
290 for (
unsigned int i = 0; i < nbData; ++i) {
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];
313 const unsigned int nbCols = 3, nbRows = 3;
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];
329 const unsigned int nbCols = 3, nbRows = 3;
331 for (
unsigned int i = 0; i < nbRows; ++i) {
332 for (
unsigned int j = 0; j < nbCols; ++j) {
349 void vpHomography::build()
351 const unsigned int nbCols = 3, nbRows = 3;
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];
364 double d = m_bP.
getD();
365 vpMatrix aHb = aRb - ((atb * n.
t()) / d);
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];
376 #ifndef DOXYGEN_SHOULD_SKIP_THIS
390 const unsigned int nbCols = 3, nbRows = 3;
394 for (
unsigned int i = 0; i < nbRows; ++i) {
396 for (
unsigned int j = 0; j < nbCols; ++j) {
397 aRb[i][j] = aMb[i][j];
403 double d = bP.
getD();
404 vpMatrix aHb_ = aRb - ((atb * n.
t()) / d);
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];
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]))));
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) {
440 double xa = iPa.
get_u();
441 double ya = iPa.
get_v();
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;
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;
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)
472 unsigned int n =
static_cast<unsigned int>(xb.size());
473 if ((yb.size() != n) || (xa.size() != n) || (ya.size() != n)) {
478 const unsigned int nbRequiredPoints = 4;
479 if (n < nbRequiredPoints) {
484 std::vector<double> xan, yan, xbn, ybn;
486 double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
491 vpHomography::hartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
492 vpHomography::hartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
501 unsigned int nbLinesA = 2;
505 vpMatrix W(nbLinesA * n, nbLinesA * n, 0);
513 for (
unsigned int i = 0; i < (nbLinesA * n); ++i) {
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];
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];
537 Y[nbLinesA * i] = xan[i];
538 Y[(nbLinesA * i) + 1] = yan[i];
543 unsigned int iter = 0;
546 while (iter < niter) {
551 residu = Y - (A * X);
557 for (
unsigned int i = 0; i < (n * nbLinesA); ++i) {
561 for (
unsigned int i = 0; i < 8; ++i) {
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)) {
582 vpHomography::hartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
590 for (
unsigned int i = 0; i < n; ++i) {
601 residual += (a - c).sumSquare();
605 residual = sqrt(residual / nbinliers);
615 double u = ipb.
get_u();
616 double v = ipb.
get_v();
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];
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);
632 const unsigned int nbRows = 3, nbCols = 3;
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];
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];
656 double u0_one_over_px = u0 * one_over_px;
657 double v0_one_over_py = v0 * one_over_py;
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);
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;
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];
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);
701 H[0][0] = A * one_over_px;
702 H[1][0] = D * one_over_px;
703 H[2][0] = h20 * one_over_px;
705 H[0][1] = B * one_over_py;
706 H[1][1] = E * one_over_py;
707 H[2][1] = h21 * one_over_py;
709 double u0_one_over_px = u0 * one_over_px;
710 double v0_one_over_py = v0 * one_over_py;
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;
Implementation of a generic 2D array used as base class for matrices and vectors.
unsigned int getCols() const
Type * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
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.
@ dimensionError
Bad dimension.
@ divideByZeroError
Division by zero.
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.
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)
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
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)
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 ...
error that can be emitted by the vpMatrix class and its derivatives
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
This class defines the container for a plane geometrical structure.
vpColVector getNormal() const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_w() const
Get the point w coordinate in the image plane.
void set_x(double x)
Set the point x coordinate in the image plane.
double get_y() const
Get the point y coordinate in the image plane.
double get_x() const
Get the point x coordinate in the image plane.
void set_y(double y)
Set the point y coordinate in the image plane.
void set_w(double w)
Set the point w coordinate in the image plane.
Implementation of a pose vector and operations on poses.
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
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.