37#include <visp3/core/vpExponentialMap.h>
38#include <visp3/core/vpHomogeneousMatrix.h>
39#include <visp3/core/vpMath.h>
40#include <visp3/core/vpMatrix.h>
41#include <visp3/core/vpPlane.h>
42#include <visp3/core/vpPoint.h>
43#include <visp3/core/vpRobust.h>
44#include <visp3/vision/vpHomography.h>
46const double vpHomography::threshold_rotation = 1e-7;
47const double vpHomography::threshold_displacement = 1e-18;
49#ifndef DOXYGEN_SHOULD_SKIP_THIS
55 double s = sqrt(dx[0] * dx[0] + dx[1] * dx[1] + dx[2] * dx[2]);
59 for (
unsigned int i = 0; i < 3; i++)
63 double mcosi = 1 - cosi;
64 rd[0][0] = cosi + mcosi * u[0] * u[0];
65 rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1];
66 rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2];
67 rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0];
68 rd[1][1] = cosi + mcosi * u[1] * u[1];
69 rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2];
70 rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0];
71 rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1];
72 rd[2][2] = cosi + mcosi * u[2] * u[2];
74 for (
unsigned int i = 0; i < 3; i++) {
75 for (
unsigned int j = 0; j < 3; j++)
108 for (
unsigned int i = 0; i < nbpoint; i++) {
110 if ((std::fabs(c2P[i].get_x() + 1) > std::fabs(
vpMath::maximum(c2P[i].get_x(), 1.))) &&
111 (std::fabs(c1P[i].get_x() + 1) > std::fabs(
vpMath::maximum(c1P[i].get_x(), 1.)))) {
115 if ((!only_1) && (!only_2))
122 robust.setMinMedianAbsoluteDeviation(0.00001);
133 for (
unsigned int i = 0; i < 3; i++)
134 for (
unsigned int j = 0; j < 3; j++)
135 c2Rc1[i][j] = c2Mc1[i][j];
139 for (
unsigned int i = 0; i < nbpoint; i++) {
141 if ((std::fabs(c2P[i].get_x() + 1) > std::fabs(
vpMath::maximum(c2P[i].get_x(), 1.))) &&
142 (std::fabs(c1P[i].get_x() + 1) > std::fabs(
vpMath::maximum(c1P[i].get_x(), 1.)))) {
143 p2[0] = c2P[i].
get_x();
144 p2[1] = c2P[i].
get_y();
146 p1[0] = c1P[i].
get_x();
147 p1[1] = c1P[i].
get_y();
150 Hp2 = c2Rc1.t() * p2;
161 H2[0][1] = -(1 + x * x);
163 H2[1][0] = 1 + y * y;
170 e2[0] = Hp2[0] - c1P[i].
get_x();
171 e2[1] = Hp2[1] - c1P[i].
get_y();
178 H1[0][1] = -(1 + x * x);
180 H1[1][0] = 1 + y * y;
185 e1[0] = Hp1[0] - c2P[i].
get_x();
186 e1[1] = Hp1[1] - c2P[i].
get_y();
221 for (
unsigned int l = 0; l < n; l++) {
227 for (
unsigned int l = 0; l < n; l++) {
228 W[2 * l][2 * l] = w[l];
229 W[2 * l + 1][2 * l + 1] = w[l];
232 for (
unsigned int l = 0; l < 2 * n; l++)
236 (L).pseudoInverse(Lp, 1e-6);
240 c2rc1 = -2 * Lp * W * e;
241 for (
unsigned int i = 0; i < 3; i++)
246 updatePoseRotation(c2rc1, c2Mc1);
249 if ((W * e).sumSquare() < 1e-10)
258 return (W * e).sumSquare();
263 double A1 = cMo[0][0] * oN.
getA() + cMo[0][1] * oN.
getB() + cMo[0][2] * oN.
getC();
264 double B1 = cMo[1][0] * oN.
getA() + cMo[1][1] * oN.
getB() + cMo[1][2] * oN.
getC();
265 double C1 = cMo[2][0] * oN.
getA() + cMo[2][1] * oN.
getB() + cMo[2][2] * oN.
getC();
266 double D1 = oN.
getD() - (cMo[0][3] * A1 + cMo[1][3] * B1 + cMo[2][3] * C1);
305 robust.setMinMedianAbsoluteDeviation(0.00001);
314 while (
vpMath::equal(r_1, r, threshold_displacement) ==
false) {
330 getPlaneInfo(oN, c1Mo, N1, d1);
331 getPlaneInfo(oN, c2Mo, N2, d2);
335 for (
unsigned int i = 0; i < nbpoint; i++) {
336 p2[0] = c2P[i].
get_x();
337 p2[1] = c2P[i].
get_y();
339 p1[0] = c1P[i].
get_x();
340 p1[1] = c1P[i].
get_y();
345 Hp2 = ((
vpMatrix)c1Rc2 + (c1Tc2 * N2.
t()) / d2) * p2;
346 Hp1 = ((
vpMatrix)c2Rc1 + (c2Tc1 * N1.
t()) / d1) * p1;
356 Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1;
365 H2[0][4] = -(1 + x * x);
367 H2[1][3] = 1 + y * y;
375 for (
unsigned int k_ = 0; k_ < 3; k_++)
376 for (
unsigned int l = 0; l < 3; l++) {
377 c1CFc2[k_][l] = c1Rc2[k_][l];
378 c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l];
379 c1CFc2[k_][l + 3] = sTR[k_][l];
385 e2[0] = Hp2[0] - c1P[i].
get_x();
386 e2[1] = Hp2[1] - c1P[i].
get_y();
391 Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2;
400 H1[0][4] = -(1 + x * x);
402 H1[1][3] = 1 + y * y;
407 e1[0] = Hp1[0] - c2P[i].
get_x();
408 e1[1] = Hp1[1] - c2P[i].
get_y();
442 for (
unsigned int l = 0; l < n; l++) {
448 for (
unsigned int l = 0; l < n; l++) {
449 W[2 * l][2 * l] = w[l];
450 W[2 * l + 1][2 * l + 1] = w[l];
453 for (
unsigned int l = 0; l < 2 * n; l++)
456 (W * L).pseudoInverse(Lp, 1e-16);
460 c2Tcc1 = -1 * Lp * W * e;
466 r = (W * e).sumSquare();
480 return (W * e).sumSquare();
515 robust.setMinMedianAbsoluteDeviation(0.00001);
524 while (
vpMath::equal(r_1, r, threshold_displacement) ==
false) {
542 for (i = 0; i < nbpoint; i++) {
543 getPlaneInfo(oN[i], c1Mo, N1, d1);
544 getPlaneInfo(oN[i], c2Mo, N2, d2);
545 p2[0] = c2P[i].
get_x();
546 p2[1] = c2P[i].
get_y();
548 p1[0] = c1P[i].
get_x();
549 p1[1] = c1P[i].
get_y();
554 Hp2 = ((
vpMatrix)c1Rc2 + (c1Tc2 * N2.
t()) / d2) * p2;
555 Hp1 = ((
vpMatrix)c2Rc1 + (c2Tc1 * N1.
t()) / d1) * p1;
565 Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1;
574 H2[0][4] = -(1 + x * x);
576 H2[1][3] = 1 + y * y;
584 for (
unsigned int k_ = 0; k_ < 3; k_++)
585 for (
unsigned int l = 0; l < 3; l++) {
586 c1CFc2[k_][l] = c1Rc2[k_][l];
587 c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l];
588 c1CFc2[k_][l + 3] = sTR[k_][l];
594 e2[0] = Hp2[0] - c1P[i].
get_x();
595 e2[1] = Hp2[1] - c1P[i].
get_y();
600 Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2;
609 H1[0][4] = -(1 + x * x);
611 H1[1][3] = 1 + y * y;
616 e1[0] = Hp1[0] - c2P[i].
get_x();
617 e1[1] = Hp1[1] - c2P[i].
get_y();
651 for (
unsigned int k_ = 0; k_ < n; k_++) {
657 for (
unsigned int k_ = 0; k_ < n; k_++) {
658 W[2 * k_][2 * k_] = w[k_];
659 W[2 * k_ + 1][2 * k_ + 1] = w[k_];
662 for (
unsigned int k_ = 0; k_ < 2 * n; k_++)
665 (W * L).pseudoInverse(Lp, 1e-16);
669 c2Tcc1 = -1 * Lp * W * e;
675 r = (W * e).sumSquare();
689 return (W * e).sumSquare();
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
static vpHomogeneousMatrix direct(const vpColVector &v)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
void extract(vpRotationMatrix &R) const
void insert(const vpRotationMatrix &R)
void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n)
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)
static Type maximum(const Type &a, const Type &b)
static double sqr(double x)
static bool equal(double x, double y, double threshold=0.001)
Implementation of a matrix and operations on matrices.
void stack(const vpMatrix &A)
This class defines the container for a plane geometrical structure.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
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.
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.