48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpMatrix.h>
50 #include <visp3/core/vpRobust.h>
51 #include <visp3/vision/vpHomography.h>
54 #include <visp3/core/vpException.h>
55 #include <visp3/core/vpMatrixException.h>
118 aMb.
buildFrom(arb[0], arb[1], arb[2], arb[3], arb[4], arb[5]);
158 void vpHomography::insert(
const vpPlane &p) { this->bP = p; }
182 for (
unsigned int i = 0; i < 3; i++)
183 for (
unsigned int j = 0; j < 3; j++)
184 H[i][j] = Minv[i][j];
228 for (
unsigned int i = 0; i < 3; i++) {
229 for (
unsigned int j = 0; j < 3; j++) {
231 for (
unsigned int k = 0; k < 3; k++) {
232 s += (*this)[i][k] * H[k][j];
252 for (
unsigned int i = 0; i < 3; i++) {
254 for (
unsigned int j = 0; j < 3; j++)
255 a[i] += (*
this)[i][j] * b[j];
279 for (
unsigned int i = 0; i < 9; i++) {
304 v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2];
305 v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2];
306 v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2];
331 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
336 for (
unsigned int i = 0; i < 9; i++) {
347 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
352 for (
unsigned int i = 0; i < 9; i++)
366 for (
unsigned int i = 0; i < 3; i++)
367 for (
unsigned int j = 0; j < 3; j++)
368 (*
this)[i][j] = H[i][j];
385 for (
unsigned int i = 0; i < 3; i++)
386 for (
unsigned int j = 0; j < 3; j++)
387 (*
this)[i][j] = H[i][j];
403 for (
unsigned int i = 0; i < 3; i++)
404 for (
unsigned int j = 0; j < 3; j++) {
419 void vpHomography::build()
424 for (
unsigned int i = 0; i < 3; i++) {
426 for (
unsigned int j = 0; j < 3; j++)
427 aRb[i][j] = aMb[i][j];
432 double d = bP.
getD();
437 for (
unsigned int i = 0; i < 3; i++)
438 for (
unsigned int j = 0; j < 3; j++)
439 (*
this)[i][j] = aHb[i][j];
455 for (
unsigned int i = 0; i < 3; i++) {
457 for (
unsigned int j = 0; j < 3; j++)
458 aRb[i][j] = aMb[i][j];
463 double d = bP.
getD();
468 for (
unsigned int i = 0; i < 3; i++)
469 for (
unsigned int j = 0; j < 3; j++)
470 aHb[i][j] = aHb_[i][j];
478 return ((*
this)[0][0] * ((*
this)[1][1] * (*
this)[2][2] - (*
this)[1][2] * (*
this)[2][1]) -
479 (*
this)[0][1] * ((*
this)[1][0] * (*
this)[2][2] - (*
this)[1][2] * (*
this)[2][0]) +
480 (*
this)[0][2] * ((*
this)[1][0] * (*
this)[2][1] - (*
this)[1][1] * (*
this)[2][0]));
489 for (
unsigned int i = 0; i < 3; i++)
490 for (
unsigned int j = 0; j < 3; j++)
497 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
521 double xa = iPa.
get_u();
522 double ya = iPa.
get_v();
524 double z = xa * bGa[2][0] + ya * bGa[2][1] + bGa[2][2];
525 double xb = (xa * bGa[0][0] + ya * bGa[0][1] + bGa[0][2]) / z;
526 double yb = (xa * bGa[1][0] + ya * bGa[1][1] + bGa[1][2]) / z;
547 double xa = Pa.
get_x();
548 double ya = Pa.
get_y();
549 double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2];
550 double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z;
551 double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z;
593 void vpHomography::robust(
const std::vector<double> &xb,
const std::vector<double> &yb,
const std::vector<double> &xa,
594 const std::vector<double> &ya,
vpHomography &aHb, std::vector<bool> &inliers,
595 double &residual,
double weights_threshold,
unsigned int niter,
bool normalization)
597 unsigned int n = (
unsigned int)xb.size();
598 if (yb.size() != n || xa.size() != n || ya.size() != n)
606 std::vector<double> xan, yan, xbn, ybn;
608 double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
613 vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
614 vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
622 unsigned int nbLinesA = 2;
626 vpMatrix W(nbLinesA * n, nbLinesA * n, 0);
634 for (
unsigned int i = 0; i < nbLinesA * n; i++) {
639 for (
unsigned int i = 0; i < n; i++) {
640 A[nbLinesA * i][0] = xbn[i];
641 A[nbLinesA * i][1] = ybn[i];
642 A[nbLinesA * i][2] = 1;
643 A[nbLinesA * i][3] = 0;
644 A[nbLinesA * i][4] = 0;
645 A[nbLinesA * i][5] = 0;
646 A[nbLinesA * i][6] = -xbn[i] * xan[i];
647 A[nbLinesA * i][7] = -ybn[i] * xan[i];
649 A[nbLinesA * i + 1][0] = 0;
650 A[nbLinesA * i + 1][1] = 0;
651 A[nbLinesA * i + 1][2] = 0;
652 A[nbLinesA * i + 1][3] = xbn[i];
653 A[nbLinesA * i + 1][4] = ybn[i];
654 A[nbLinesA * i + 1][5] = 1;
655 A[nbLinesA * i + 1][6] = -xbn[i] * yan[i];
656 A[nbLinesA * i + 1][7] = -ybn[i] * yan[i];
658 Y[nbLinesA * i] = xan[i];
659 Y[nbLinesA * i + 1] = yan[i];
664 unsigned int iter = 0;
667 while (iter < niter) {
678 for (
unsigned int i = 0; i < n * nbLinesA; i++) {
682 for (
unsigned int i = 0; i < 8; i++)
689 unsigned int nbinliers = 0;
690 for (
unsigned int i = 0; i < n; i++) {
691 if (w[i * 2] < weights_threshold && w[i * 2 + 1] < weights_threshold)
701 vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
708 for (
unsigned int i = 0; i < n; i++) {
719 residual += (a - c).sumSquare();
723 residual = sqrt(residual / nbinliers);
739 double u = ipb.
get_u();
740 double v = ipb.
get_v();
742 double u_a = (*this)[0][0] * u + (*this)[0][1] * v + (*this)[0][2];
743 double v_a = (*this)[1][0] * u + (*this)[1][1] * v + (*this)[1][2];
744 double w_a = (*this)[2][0] * u + (*this)[2][1] * v + (*this)[2][2];
746 if (std::fabs(w_a) > std::numeric_limits<double>::epsilon()) {
747 ipa.
set_u(u_a / w_a);
748 ipa.
set_v(v_a / w_a);
761 for (
unsigned int i = 0; i < 3; i++)
762 for (
unsigned int j = 0; j < 3; j++)
763 M[i][j] = (*
this)[i][j];
795 double h00 = (*this)[0][0], h01 = (*this)[0][1], h02 = (*this)[0][2];
796 double h10 = (*this)[1][0], h11 = (*this)[1][1], h12 = (*this)[1][2];
797 double h20 = (*this)[2][0], h21 = (*this)[2][1], h22 = (*this)[2][2];
799 double u0_one_over_px = u0 * one_over_px;
800 double v0_one_over_py = v0 * one_over_py;
802 double A = h00 * one_over_px - h20 * u0_one_over_px;
803 double B = h01 * one_over_px - h21 * u0_one_over_px;
804 double C = h02 * one_over_px - h22 * u0_one_over_px;
805 double D = h10 * one_over_py - h20 * v0_one_over_py;
806 double E = h11 * one_over_py - h21 * v0_one_over_py;
807 double F = h12 * one_over_py - h22 * v0_one_over_py;
817 H[0][2] = A * u0 + B * v0 + C;
818 H[1][2] = D * u0 + E * v0 + F;
819 H[2][2] = h20 * u0 + h21 * v0 + h22;
851 double h00 = (*this)[0][0], h01 = (*this)[0][1], h02 = (*this)[0][2];
852 double h10 = (*this)[1][0], h11 = (*this)[1][1], h12 = (*this)[1][2];
853 double h20 = (*this)[2][0], h21 = (*this)[2][1], h22 = (*this)[2][2];
855 double A = h00 * px + u0 * h20;
856 double B = h01 * px + u0 * h21;
857 double C = h02 * px + u0 * h22;
858 double D = h10 * py + v0 * h20;
859 double E = h11 * py + v0 * h21;
860 double F = h12 * py + v0 * h22;
862 H[0][0] = A * one_over_px;
863 H[1][0] = D * one_over_px;
864 H[2][0] = h20 * one_over_px;
866 H[0][1] = B * one_over_py;
867 H[1][1] = E * one_over_py;
868 H[2][1] = h21 * one_over_py;
870 double u0_one_over_px = u0 * one_over_px;
871 double v0_one_over_py = v0 * one_over_py;
873 H[0][2] = -A * u0_one_over_px - B * v0_one_over_py + C;
874 H[1][2] = -D * u0_one_over_px - E * v0_one_over_py + F;
875 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 emited by ViSP classes.
@ dimensionError
Bad dimension.
@ divideByZeroError
Division by zero.
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
invert the homography
vpImagePoint projection(const vpImagePoint &p)
void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP)
Construction from Translation and rotation and a plane.
vpHomography()
initialize an homography as Identity
vpHomography & operator/=(double v)
Divide all the element of the homography matrix by v : Hij = Hij / v.
void save(std::ofstream &f) const
vpHomography operator*(const vpHomography &H) const
vpHomography operator/(const double &v) const
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 > &inlier, double &residual, double weights_threshold=0.4, unsigned int niter=4, bool normalization=true)
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)
Load an homography from a file.
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 ...
error that can be emited by the vpMatrix class and its derivates
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.