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>
102 aMb.
buildFrom(arb[0], arb[1], arb[2], arb[3], arb[4], arb[5]);
121 void vpHomography::insert(
const vpPlane &p) { this->bP = p; }
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];
155 for (
unsigned int i = 0; i < 3; i++) {
156 for (
unsigned int j = 0; j < 3; j++) {
158 for (
unsigned int k = 0; k < 3; k++) {
159 s += (*this)[i][k] * H[k][j];
174 for (
unsigned int i = 0; i < 3; i++) {
176 for (
unsigned int j = 0; j < 3; j++)
177 a[i] += (*
this)[i][j] * b[j];
187 for (
unsigned int i = 0; i < 9; i++) {
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];
218 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
223 for (
unsigned int i = 0; i < 9; i++) {
233 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
238 for (
unsigned int i = 0; i < 9; i++)
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];
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];
270 for (
unsigned int i = 0; i < 3; i++)
271 for (
unsigned int j = 0; j < 3; j++) {
286 void vpHomography::build()
291 for (
unsigned int i = 0; i < 3; i++) {
293 for (
unsigned int j = 0; j < 3; j++)
294 aRb[i][j] = aMb[i][j];
299 double d = bP.
getD();
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];
322 for (
unsigned int i = 0; i < 3; i++) {
324 for (
unsigned int j = 0; j < 3; j++)
325 aRb[i][j] = aMb[i][j];
330 double d = bP.
getD();
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];
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]));
349 for (
unsigned int i = 0; i < 3; i++)
350 for (
unsigned int j = 0; j < 3; j++)
357 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
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;
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;
397 void 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)
401 unsigned int n = (
unsigned int)xb.size();
402 if (yb.size() != n || xa.size() != n || ya.size() != n)
410 std::vector<double> xan, yan, xbn, ybn;
412 double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
417 vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
418 vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
426 unsigned int nbLinesA = 2;
430 vpMatrix W(nbLinesA * n, nbLinesA * n, 0);
438 for (
unsigned int i = 0; i < nbLinesA * n; i++) {
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];
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];
462 Y[nbLinesA * i] = xan[i];
463 Y[nbLinesA * i + 1] = yan[i];
468 unsigned int iter = 0;
471 while (iter < niter) {
482 for (
unsigned int i = 0; i < n * nbLinesA; i++) {
486 for (
unsigned int i = 0; i < 8; i++)
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)
505 vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
512 for (
unsigned int i = 0; i < n; i++) {
523 residual += (a - c).sumSquare();
527 residual = sqrt(residual / nbinliers);
536 double u = ipb.
get_u();
537 double v = ipb.
get_v();
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];
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);
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];
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];
574 double u0_one_over_px = u0 * one_over_px;
575 double v0_one_over_py = v0 * one_over_py;
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;
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;
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];
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;
619 H[0][0] = A * one_over_px;
620 H[1][0] = D * one_over_px;
621 H[2][0] = h20 * one_over_px;
623 H[0][1] = B * one_over_py;
624 H[1][1] = E * one_over_py;
625 H[2][1] = h21 * one_over_py;
627 double u0_one_over_px = u0 * one_over_px;
628 double v0_one_over_py = v0 * one_over_py;
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;
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.
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)
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)
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.