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; }
173 for (
unsigned int i = 0; i < 3; i++)
174 for (
unsigned int j = 0; j < 3; j++)
175 H[i][j] = Minv[i][j];
219 for (
unsigned int i = 0; i < 3; i++) {
220 for (
unsigned int j = 0; j < 3; j++) {
222 for (
unsigned int k = 0; k < 3; k++) {
223 s += (*this)[i][k] * H[k][j];
243 for (
unsigned int i = 0; i < 3; i++) {
245 for (
unsigned int j = 0; j < 3; j++)
246 a[i] += (*
this)[i][j] * b[j];
270 for (
unsigned int i = 0; i < 9; i++) {
295 v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2];
296 v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2];
297 v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2];
322 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
327 for (
unsigned int i = 0; i < 9; i++) {
338 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
343 for (
unsigned int i = 0; i < 9; i++)
357 for (
unsigned int i = 0; i < 3; i++)
358 for (
unsigned int j = 0; j < 3; j++)
359 (*
this)[i][j] = H[i][j];
376 for (
unsigned int i = 0; i < 3; i++)
377 for (
unsigned int j = 0; j < 3; j++)
378 (*
this)[i][j] = H[i][j];
394 for (
unsigned int i = 0; i < 3; i++)
395 for (
unsigned int j = 0; j < 3; j++) {
410 void vpHomography::build()
415 for (
unsigned int i = 0; i < 3; i++) {
417 for (
unsigned int j = 0; j < 3; j++)
418 aRb[i][j] = aMb[i][j];
423 double d = bP.
getD();
428 for (
unsigned int i = 0; i < 3; i++)
429 for (
unsigned int j = 0; j < 3; j++)
430 (*
this)[i][j] = aHb[i][j];
446 for (
unsigned int i = 0; i < 3; i++) {
448 for (
unsigned int j = 0; j < 3; j++)
449 aRb[i][j] = aMb[i][j];
454 double d = bP.
getD();
459 for (
unsigned int i = 0; i < 3; i++)
460 for (
unsigned int j = 0; j < 3; j++)
461 aHb[i][j] = aHb_[i][j];
470 for (
unsigned int i = 0; i < 3; i++)
471 for (
unsigned int j = 0; j < 3; j++)
478 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) 486 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) 502 double xa = iPa.
get_u();
503 double ya = iPa.
get_v();
505 double z = xa * H[2][0] + ya * H[2][1] + H[2][2];
506 double xb = (xa * H[0][0] + ya * H[0][1] + H[0][2]) / z;
507 double yb = (xa * H[1][0] + ya * H[1][1] + H[1][2]) / z;
528 double xa = Pa.
get_x();
529 double ya = Pa.
get_y();
530 double z = xa * bHa[2][0] + ya * bHa[2][1] + bHa[2][2];
531 double xb = (xa * bHa[0][0] + ya * bHa[0][1] + bHa[0][2]) / z;
532 double yb = (xa * bHa[1][0] + ya * bHa[1][1] + bHa[1][2]) / z;
571 void vpHomography::robust(
const std::vector<double> &xb,
const std::vector<double> &yb,
const std::vector<double> &xa,
572 const std::vector<double> &ya,
vpHomography &aHb, std::vector<bool> &inliers,
573 double &residual,
double weights_threshold,
unsigned int niter,
bool normalization)
575 unsigned int n = (
unsigned int)xb.size();
576 if (yb.size() != n || xa.size() != n || ya.size() != n)
584 std::vector<double> xan, yan, xbn, ybn;
586 double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
591 vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
592 vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
600 unsigned int nbLinesA = 2;
604 vpMatrix W(nbLinesA * n, nbLinesA * n);
612 for (
unsigned int i = 0; i < nbLinesA * n; i++) {
617 for (
unsigned int i = 0; i < n; i++) {
618 A[nbLinesA * i][0] = xbn[i];
619 A[nbLinesA * i][1] = ybn[i];
620 A[nbLinesA * i][2] = 1;
621 A[nbLinesA * i][3] = 0;
622 A[nbLinesA * i][4] = 0;
623 A[nbLinesA * i][5] = 0;
624 A[nbLinesA * i][6] = -xbn[i] * xan[i];
625 A[nbLinesA * i][7] = -ybn[i] * xan[i];
627 A[nbLinesA * i + 1][0] = 0;
628 A[nbLinesA * i + 1][1] = 0;
629 A[nbLinesA * i + 1][2] = 0;
630 A[nbLinesA * i + 1][3] = xbn[i];
631 A[nbLinesA * i + 1][4] = ybn[i];
632 A[nbLinesA * i + 1][5] = 1;
633 A[nbLinesA * i + 1][6] = -xbn[i] * yan[i];
634 A[nbLinesA * i + 1][7] = -ybn[i] * yan[i];
636 Y[nbLinesA * i] = xan[i];
637 Y[nbLinesA * i + 1] = yan[i];
642 unsigned int iter = 0;
645 while (iter < niter) {
657 for (
unsigned int i = 0; i < n * nbLinesA; i++) {
661 for (
unsigned int i = 0; i < 8; i++)
666 aHbnorm /= aHbnorm[2][2];
672 unsigned int nbinliers = 0;
673 for (
unsigned int i = 0; i < n; i++) {
674 if (w[i * 2] < weights_threshold && w[i * 2 + 1] < weights_threshold)
684 vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
691 for (
unsigned int i = 0; i < n; i++) {
702 residual += (a - c).sumSquare();
706 residual = sqrt(residual / nbinliers);
722 double u = ipb.
get_u();
723 double v = ipb.
get_v();
725 double u_a = (*this)[0][0] * u + (*this)[0][1] * v + (*this)[0][2];
726 double v_a = (*this)[1][0] * u + (*this)[1][1] * v + (*this)[1][2];
727 double w_a = (*this)[2][0] * u + (*this)[2][1] * v + (*this)[2][2];
729 if (std::fabs(w_a) > std::numeric_limits<double>::epsilon()) {
730 ipa.
set_u(u_a / w_a);
731 ipa.
set_v(v_a / w_a);
744 for (
unsigned int i = 0; i < 3; i++)
745 for (
unsigned int j = 0; j < 3; j++)
746 M[i][j] = (*
this)[i][j];
vpMatrix get_K_inverse() const
void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP)
Construction from Translation and rotation and a plane.
Implementation of a matrix and operations on matrices.
vpHomography & operator=(const vpHomography &H)
vpHomography & operator/=(double v)
Divide all the element of the homography matrix by v : Hij = Hij / v.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa)
Implementation of an homogeneous matrix and operations on such kind of matrices.
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.
void set_x(const double x)
Set the point x coordinate in the image plane.
Type * data
Address of the first element of the data array.
Implementation of a generic 2D array used as vase class of matrices and vectors.
vpImagePoint projection(const vpImagePoint &p)
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getCols() const
double get_y() const
Get the point y coordinate in the image plane.
double get_w() const
Get the point w coordinate in the image plane.
Class that defines what is a point.
Implementation of a rotation matrix and operations on such kind of matrices.
void insert(const vpRotationMatrix &R)
Implementation of an homography and operations on homographies.
void set_u(const double u)
vpHomography()
initialize an homography as Identity
void set_v(const double v)
void load(std::ifstream &f)
Load an homography from a file.
Generic class defining intrinsic camera parameters.
void set_y(const double y)
Set the point y coordinate in the image plane.
void set_w(const double w)
Set the point w coordinate in the image plane.
double get_x() const
Get the point x coordinate in the image plane.
unsigned int getRows() const
vpColVector getNormal() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpHomography operator/(const double &v) const
void save(std::ofstream &f) 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)
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpHomography inverse() const
invert the homography
Contains an M-Estimator and various influence function.
error that can be emited by the vpMatrix class and its derivates
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
This class defines the container for a plane geometrical structure.
void convert(std::vector< float > &M)
vpHomography operator*(const vpHomography &H) const
void setIteration(const unsigned int iter)
Set iteration.
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.