44 #include <visp3/core/vpExponentialMap.h> 45 #include <visp3/core/vpPoint.h> 46 #include <visp3/core/vpRobust.h> 47 #include <visp3/vision/vpPose.h> 60 double residu_1 = 1e8;
67 unsigned int nb = (
unsigned int)
listP.size();
74 std::list<vpPoint> lP;
78 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
80 sd[2 * k] = P.
get_x();
81 sd[2 * k + 1] = P.
get_y();
90 while (std::fabs(residu_1 - r) > vvsEpsilon) {
95 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
102 double x = s[2 * k] = P.
get_x();
103 double y = s[2 * k + 1] = P.
get_y();
104 double Z = P.
get_Z();
105 L[2 * k][0] = -1 / Z;
109 L[2 * k][4] = -(1 + x * x);
113 L[2 * k + 1][1] = -1 / Z;
114 L[2 * k + 1][2] = y / Z;
115 L[2 * k + 1][3] = 1 + y * y;
116 L[2 * k + 1][4] = -x * y;
117 L[2 * k + 1][5] = -x;
139 if (iter++ > vvsIterMax) {
144 if (computeCovariance)
165 double residu_1 = 1e8;
174 unsigned int nb = (
unsigned int)
listP.size();
182 std::list<vpPoint> lP;
186 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
188 sd[2 * k_] = P.
get_x();
189 sd[2 * k_ + 1] = P.
get_y();
200 while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
205 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
212 double x = s[2 * k_] = P.
get_x();
213 double y = s[2 * k_ + 1] = P.
get_y();
214 double Z = P.
get_Z();
215 L[2 * k_][0] = -1 / Z;
217 L[2 * k_][2] = x / Z;
218 L[2 * k_][3] = x * y;
219 L[2 * k_][4] = -(1 + x * x);
222 L[2 * k_ + 1][0] = 0;
223 L[2 * k_ + 1][1] = -1 / Z;
224 L[2 * k_ + 1][2] = y / Z;
225 L[2 * k_ + 1][3] = 1 + y * y;
226 L[2 * k_ + 1][4] = -x * y;
227 L[2 * k_ + 1][5] = -x;
236 for (
unsigned int k = 0; k < error.
getRows() / 2; k++) {
243 for (
unsigned int k = 0; k < error.
getRows() / 2; k++) {
244 W[2 * k][2 * k] = w[k];
245 W[2 * k + 1][2 * k + 1] = w[k];
249 (W * L).pseudoInverse(Lp, 1e-6);
252 v = -
lambda * Lp * W * error;
256 if (iter++ > vvsIterMax)
260 if (computeCovariance)
Implementation of a matrix and operations on matrices.
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
void poseVirtualVSrobust(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach and a robust control law. ...
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)
void track(const vpHomogeneousMatrix &cMo)
double get_y() const
Get the point y coordinate in the image plane.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
void poseVirtualVS(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach.
Class that defines what is a point.
double lambda
parameters use for the virtual visual servoing approach
static double sqr(double x)
double get_x() const
Get the point x coordinate in the image plane.
unsigned int getRows() const
double get_Z() const
Get the point Z coordinate in the camera frame.
Implementation of column vector and the associated operations.
vpHomogeneousMatrix inverse() const
static vpHomogeneousMatrix direct(const vpColVector &v)
Contains an M-Estimator and various influence function.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
void setThreshold(const double noise_threshold)
void setIteration(const unsigned int iter)
Set iteration.
void resize(const unsigned int i, const bool flagNullify=true)