41 #include <visp3/core/vpConfig.h>
42 #include <visp3/core/vpExponentialMap.h>
43 #include <visp3/core/vpPoint.h>
44 #include <visp3/core/vpRobust.h>
45 #include <visp3/vision/vpPose.h>
57 double residu_1 = 1e8;
64 unsigned int nb = (
unsigned int)
listP.size();
71 std::list<vpPoint> lP;
75 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
77 sd[2 * k] = P.
get_x();
78 sd[2 * k + 1] = P.
get_y();
87 while (std::fabs(residu_1 - r) > vvsEpsilon) {
92 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
99 double x = s[2 * k] = P.
get_x();
100 double y = s[2 * k + 1] = P.
get_y();
101 double Z = P.
get_Z();
102 L[2 * k][0] = -1 / Z;
106 L[2 * k][4] = -(1 + x * x);
110 L[2 * k + 1][1] = -1 / Z;
111 L[2 * k + 1][2] = y / Z;
112 L[2 * k + 1][3] = 1 + y * y;
113 L[2 * k + 1][4] = -x * y;
114 L[2 * k + 1][5] = -x;
125 L.pseudoInverse(Lp, 1e-16);
136 if (iter++ > vvsIterMax) {
141 if (computeCovariance)
162 double residu_1 = 1e8;
171 unsigned int nb = (
unsigned int)
listP.size();
178 std::list<vpPoint> lP;
182 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
184 sd[2 * k_] = P.
get_x();
185 sd[2 * k_ + 1] = P.
get_y();
196 while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
201 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
208 double x = s[2 * k_] = P.
get_x();
209 double y = s[2 * k_ + 1] = P.
get_y();
210 double Z = P.
get_Z();
211 L[2 * k_][0] = -1 / Z;
213 L[2 * k_][2] = x / Z;
214 L[2 * k_][3] = x * y;
215 L[2 * k_][4] = -(1 + x * x);
218 L[2 * k_ + 1][0] = 0;
219 L[2 * k_ + 1][1] = -1 / Z;
220 L[2 * k_ + 1][2] = y / Z;
221 L[2 * k_ + 1][3] = 1 + y * y;
222 L[2 * k_ + 1][4] = -x * y;
223 L[2 * k_ + 1][5] = -x;
232 for (
unsigned int k = 0; k < error.
getRows() / 2; k++) {
238 for (
unsigned int k = 0; k < error.
getRows() / 2; k++) {
239 W[2 * k][2 * k] = w[k];
240 W[2 * k + 1][2 * k + 1] = w[k];
244 (W * L).pseudoInverse(Lp, 1e-6);
247 v = -
lambda * Lp * W * error;
250 if (iter++ > vvsIterMax)
254 if (computeCovariance)
265 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
266 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
278 auto residu_1{1e8}, r{1e8 - 1};
279 const auto lambda{0.9}, vvsEpsilon{1e-8};
280 const unsigned int vvsIterMax{200};
282 const unsigned int nb =
static_cast<unsigned int>(points.size());
288 for (
auto i = 0u; i < points.size(); i++) {
289 sd[3 * i] = points[i].get_x();
290 sd[3 * i + 1] = points[i].get_y();
291 sd[3 * i + 2] = points[i].get_Z();
296 while (std::fabs(residu_1 - r) > vvsEpsilon) {
300 for (
auto i = 0u; i < points.size(); i++) {
305 points.at(i).changeFrame(cMo, cP);
306 points.at(i).projection(cP, p);
308 const auto x = s[3 * i] = p[0];
309 const auto y = s[3 * i + 1] = p[1];
310 const auto Z = s[3 * i + 2] = cP[2];
311 L[3 * i][0] = -1 / Z;
319 L[3 * i + 1][1] = -1 / Z;
320 L[3 * i + 1][2] = y / Z;
322 L[3 * i + 1][4] = -x * y;
323 L[3 * i + 1][5] = -x;
327 L[3 * i + 2][2] = -1;
328 L[3 * i + 2][3] = -y * Z;
329 L[3 * i + 2][4] = x * Z;
330 L[3 * i + 2][5] = -0;
339 L.pseudoInverse(Lp, 1e-16);
342 const auto v = -
lambda * Lp * err;
347 if (iter++ > vvsIterMax) {
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int getRows() const
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
static vpHomogeneousMatrix direct(const vpColVector &v)
void track(const vpHomogeneousMatrix &cMo)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double sqr(double x)
Implementation of a matrix and operations on matrices.
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
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.
double get_Z() const
Get the point cZ coordinate in the camera frame.
void poseVirtualVS(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach.
std::list< vpPoint > listP
Array of point (use here class vpPoint)
static std::optional< vpHomogeneousMatrix > poseVirtualVSWithDepth(const std::vector< vpPoint > &points, const vpHomogeneousMatrix &cMo)
double lambda
parameters use for the virtual visual servoing approach
void poseVirtualVSrobust(vpHomogeneousMatrix &cMo)
Compute the pose using virtual visual servoing approach and a robust control law.
Contains an M-estimator and various influence function.
@ TUKEY
Tukey influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
void setMinMedianAbsoluteDeviation(double mad_min)