Visual Servoing Platform version 3.5.0
vpHomography.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Homography transformation.
33 *
34 * Authors:
35 * Muriel Pressigout
36 * Fabien Spindler
37 *
38 *****************************************************************************/
39
46#include <stdio.h>
47
48#include <visp3/core/vpDebug.h>
49#include <visp3/core/vpMatrix.h>
50#include <visp3/core/vpRobust.h>
51#include <visp3/vision/vpHomography.h>
52
53// Exception
54#include <visp3/core/vpException.h>
55#include <visp3/core/vpMatrixException.h>
56
60vpHomography::vpHomography() : vpArray2D<double>(3, 3), aMb(), bP() { eye(); }
61
66vpHomography::vpHomography(const vpHomography &H) : vpArray2D<double>(3, 3), aMb(), bP() { *this = H; }
67
71vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p) : vpArray2D<double>(3, 3), aMb(), bP()
72{
73 buildFrom(M, p);
74}
75
77 : vpArray2D<double>(3, 3), aMb(), bP()
78{
79 buildFrom(tu, atb, p);
80}
81
83 : vpArray2D<double>(3, 3), aMb(), bP()
84{
85 buildFrom(aRb, atb, p);
86}
87
88vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : vpArray2D<double>(3, 3), aMb(), bP()
89{
90 buildFrom(arb, p);
91}
92
94{
95 insert(M);
96 insert(p);
97 build();
98}
99
101{
102 insert(tu);
103 insert(atb);
104 insert(p);
105 build();
106}
107
109{
110 insert(aRb);
111 insert(atb);
112 insert(p);
113 build();
114}
115
117{
118 aMb.buildFrom(arb[0], arb[1], arb[2], arb[3], arb[4], arb[5]);
119 insert(p);
120 build();
121}
122
123/*********************************************************************/
124
129void vpHomography::insert(const vpRotationMatrix &aRb) { aMb.insert(aRb); }
130
135void vpHomography::insert(const vpHomogeneousMatrix &M) { this->aMb = M; }
136
142void vpHomography::insert(const vpThetaUVector &tu)
143{
144 vpRotationMatrix aRb(tu);
145 aMb.insert(aRb);
146}
147
152void vpHomography::insert(const vpTranslationVector &atb) { aMb.insert(atb); }
153
158void vpHomography::insert(const vpPlane &p) { this->bP = p; }
159
171vpHomography vpHomography::inverse(double sv_threshold, unsigned int *rank) const
172{
173 vpMatrix M = (*this).convert();
174 vpMatrix Minv;
175 unsigned int r = M.pseudoInverse(Minv, sv_threshold);
176 if (rank != NULL) {
177 *rank = r;
178 }
179
180 vpHomography H;
181
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];
185
186 return H;
187}
188
194void vpHomography::inverse(vpHomography &bHa) const { bHa = inverse(); }
195
203void vpHomography::save(std::ofstream &f) const
204{
205 if (!f.fail()) {
206 f << *this;
207 } else {
208 throw(vpException(vpException::ioError, "Cannot write the homography to the output stream"));
209 }
210}
211
226{
227 vpHomography Hp;
228 for (unsigned int i = 0; i < 3; i++) {
229 for (unsigned int j = 0; j < 3; j++) {
230 double s = 0.;
231 for (unsigned int k = 0; k < 3; k++) {
232 s += (*this)[i][k] * H[k][j];
233 }
234 Hp[i][j] = s;
235 }
236 }
237 return Hp;
238}
239
246{
247 if (b.size() != 3)
248 throw(vpException(vpException::dimensionError, "Cannot multiply an homography by a vector of dimension %d",
249 b.size()));
250
251 vpColVector a(3);
252 for (unsigned int i = 0; i < 3; i++) {
253 a[i] = 0.;
254 for (unsigned int j = 0; j < 3; j++)
255 a[i] += (*this)[i][j] * b[j];
256 }
257
258 return a;
259}
260
276{
277 vpHomography H;
278
279 for (unsigned int i = 0; i < 9; i++) {
280 H.data[i] = this->data[i] * v;
281 }
282
283 return H;
284}
285
296{
297 vpPoint a_P;
298 vpColVector v(3), v1(3);
299
300 v[0] = b_P.get_x();
301 v[1] = b_P.get_y();
302 v[2] = b_P.get_w();
303
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];
307
308 // v1 = M*v ;
309 a_P.set_x(v1[0]);
310 a_P.set_y(v1[1]);
311 a_P.set_w(v1[2]);
312
313 return a_P;
314}
329{
330 vpHomography H;
331 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
332 throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
333
334 double vinv = 1 / v;
335
336 for (unsigned int i = 0; i < 9; i++) {
337 H.data[i] = this->data[i] * vinv;
338 }
339
340 return H;
341}
342
345{
346 // if (x == 0)
347 if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
348 throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
349
350 double vinv = 1 / v;
351
352 for (unsigned int i = 0; i < 9; i++)
353 data[i] *= vinv;
354
355 return *this;
356}
357
365{
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];
369
370 aMb = H.aMb;
371 bP = H.bP;
372 return *this;
373}
381{
382 if (H.getRows() != 3 || H.getCols() != 3)
383 throw(vpException(vpException::dimensionError, "The matrix is not an homography"));
384
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];
388
389 return *this;
390}
391
400void vpHomography::load(std::ifstream &f)
401{
402 if (!f.fail()) {
403 for (unsigned int i = 0; i < 3; i++)
404 for (unsigned int j = 0; j < 3; j++) {
405 f >> (*this)[i][j];
406 }
407 } else {
408 throw(vpException(vpException::ioError, "Cannot read the homography from the input stream"));
409 }
410}
411
419void vpHomography::build()
420{
421 vpColVector n(3);
422 vpColVector atb(3);
423 vpMatrix aRb(3, 3);
424 for (unsigned int i = 0; i < 3; i++) {
425 atb[i] = aMb[i][3];
426 for (unsigned int j = 0; j < 3; j++)
427 aRb[i][j] = aMb[i][j];
428 }
429
430 bP.getNormal(n);
431
432 double d = bP.getD();
433 vpMatrix aHb = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the
434 // plane equation. So if the plane is described by
435 // Ax+By+Cz+D=0, d=-D
436
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];
440}
441
450void vpHomography::build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, const vpPlane &bP)
451{
452 vpColVector n(3);
453 vpColVector atb(3);
454 vpMatrix aRb(3, 3);
455 for (unsigned int i = 0; i < 3; i++) {
456 atb[i] = aMb[i][3];
457 for (unsigned int j = 0; j < 3; j++)
458 aRb[i][j] = aMb[i][j];
459 }
460
461 bP.getNormal(n);
462
463 double d = bP.getD();
464 vpMatrix aHb_ = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the
465 // plane equation. So if the plane is described by
466 // Ax+By+Cz+D=0, d=-D
467
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];
471}
472
476double vpHomography::det() const
477{
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]));
481}
482
488{
489 for (unsigned int i = 0; i < 3; i++)
490 for (unsigned int j = 0; j < 3; j++)
491 if (i == j)
492 (*this)[i][j] = 1.0;
493 else
494 (*this)[i][j] = 0.0;
495}
496
497#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
505#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
506
520{
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;
527
528 vpImagePoint iPb(yb, xb);
529
530 return iPb;
531}
532
546{
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;
552
553 vpPoint Pb;
554 Pb.set_x(xb);
555 Pb.set_y(yb);
556
557 return Pb;
558}
559
593void 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)
596{
597 unsigned int n = (unsigned int)xb.size();
598 if (yb.size() != n || xa.size() != n || ya.size() != n)
599 throw(vpException(vpException::dimensionError, "Bad dimension for robust homography estimation"));
600
601 // 4 point are required
602 if (n < 4)
603 throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
604
605 try {
606 std::vector<double> xan, yan, xbn, ybn;
607
608 double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
609
610 vpHomography aHbn;
611
612 if (normalization) {
613 vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
614 vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
615 } else {
616 xbn = xb;
617 ybn = yb;
618 xan = xa;
619 yan = ya;
620 }
621
622 unsigned int nbLinesA = 2;
623 vpMatrix A(nbLinesA * n, 8);
624 vpColVector X(8);
625 vpColVector Y(nbLinesA * n);
626 vpMatrix W(nbLinesA * n, nbLinesA * n, 0); // Weight matrix
627
628 vpColVector w(nbLinesA * n);
629
630 // All the weights are set to 1 at the beginning to use a classical least
631 // square scheme
632 w = 1;
633 // Update the square matrix associated to the weights
634 for (unsigned int i = 0; i < nbLinesA * n; i++) {
635 W[i][i] = w[i];
636 }
637
638 // build matrix A
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];
648
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];
657
658 Y[nbLinesA * i] = xan[i];
659 Y[nbLinesA * i + 1] = yan[i];
660 }
661
662 vpMatrix WA;
663 vpMatrix WAp;
664 unsigned int iter = 0;
665 vpRobust r; // M-Estimator
666
667 while (iter < niter) {
668 WA = W * A;
669
670 X = WA.pseudoInverse(1e-26) * W * Y;
671 vpColVector residu;
672 residu = Y - A * X;
673
674 // Compute the weights using the Tukey biweight M-Estimator
675 r.MEstimator(vpRobust::TUKEY, residu, w);
676
677 // Update the weights matrix
678 for (unsigned int i = 0; i < n * nbLinesA; i++) {
679 W[i][i] = w[i];
680 }
681 // Build the homography
682 for (unsigned int i = 0; i < 8; i++)
683 aHbn.data[i] = X[i];
684 aHbn[2][2] = 1;
685
686 iter++;
687 }
688 inliers.resize(n);
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)
692 inliers[i] = false;
693 else {
694 inliers[i] = true;
695 nbinliers++;
696 }
697 }
698
699 if (normalization) {
700 // H after denormalization
701 vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
702 } else {
703 aHb = aHbn;
704 }
705
706 residual = 0;
707 vpColVector a(3), b(3), c(3);
708 for (unsigned int i = 0; i < n; i++) {
709 if (inliers[i]) {
710 a[0] = xa[i];
711 a[1] = ya[i];
712 a[2] = 1;
713 b[0] = xb[i];
714 b[1] = yb[i];
715 b[2] = 1;
716
717 c = aHb * b;
718 c /= c[2];
719 residual += (a - c).sumSquare();
720 }
721 }
722
723 residual = sqrt(residual / nbinliers);
724 } catch (...) {
725 throw(vpException(vpException::fatalError, "Cannot estimate an homography"));
726 }
727}
728
737{
738 vpImagePoint ipa;
739 double u = ipb.get_u();
740 double v = ipb.get_v();
741
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];
745
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);
749 }
750
751 return ipa;
752}
753
759{
760 vpMatrix M(3, 3);
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];
764
765 return M;
766}
767
787{
788 vpHomography H;
789 double px = cam.get_px();
790 double py = cam.get_py();
791 double u0 = cam.get_u0();
792 double v0 = cam.get_v0();
793 double one_over_px = cam.get_px_inverse();
794 double one_over_py = cam.get_py_inverse();
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];
798
799 double u0_one_over_px = u0 * one_over_px;
800 double v0_one_over_py = v0 * one_over_py;
801
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;
808
809 H[0][0] = A * px;
810 H[1][0] = D * px;
811 H[2][0] = h20 * px;
812
813 H[0][1] = B * py;
814 H[1][1] = E * py;
815 H[2][1] = h21 * py;
816
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;
820
821 return H;
822}
823
843{
844 vpHomography H;
845 double px = cam.get_px();
846 double py = cam.get_py();
847 double u0 = cam.get_u0();
848 double v0 = cam.get_v0();
849 double one_over_px = cam.get_px_inverse();
850 double one_over_py = cam.get_py_inverse();
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];
854
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;
861
862 H[0][0] = A * one_over_px;
863 H[1][0] = D * one_over_px;
864 H[2][0] = h20 * one_over_px;
865
866 H[0][1] = B * one_over_py;
867 H[1][1] = E * one_over_py;
868 H[2][1] = h21 * one_over_py;
869
870 double u0_one_over_px = u0 * one_over_px;
871 double v0_one_over_py = v0 * one_over_py;
872
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;
876
877 return H;
878}
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
unsigned int getCols() const
Definition: vpArray2D.h:279
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
unsigned int getRows() const
Definition: vpArray2D.h:289
Generic class defining intrinsic camera parameters.
double get_px_inverse() const
double get_py_inverse() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ ioError
I/O error.
Definition: vpException.h:91
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
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.
Definition: vpHomography.h:175
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.
double det() const
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
void setIdentity()
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
vpMatrix convert() 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)
Definition: vpHomography.h:266
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double get_u() const
Definition: vpImagePoint.h:262
void set_u(double u)
Definition: vpImagePoint.h:225
void set_v(double v)
Definition: vpImagePoint.h:236
double get_v() const
Definition: vpImagePoint.h:273
error that can be emited by the vpMatrix class and its derivates
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:59
double getD() const
Definition: vpPlane.h:108
vpColVector getNormal() const
Definition: vpPlane.cpp:238
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
double get_w() const
Get the point w coordinate in the image plane.
Definition: vpPoint.cpp:474
void set_x(double x)
Set the point x coordinate in the image plane.
Definition: vpPoint.cpp:511
double get_y() const
Get the point y coordinate in the image plane.
Definition: vpPoint.cpp:472
double get_x() const
Get the point x coordinate in the image plane.
Definition: vpPoint.cpp:470
void set_y(double y)
Set the point y coordinate in the image plane.
Definition: vpPoint.cpp:513
void set_w(double w)
Set the point w coordinate in the image plane.
Definition: vpPoint.cpp:515
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Contains an M-estimator and various influence function.
Definition: vpRobust.h:89
@ TUKEY
Tukey influence function.
Definition: vpRobust.h:93
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
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.
vpRowVector t() const