39#include <visp3/core/vpConfig.h>
40#include <visp3/core/vpExponentialMap.h>
41#include <visp3/core/vpPoint.h>
42#include <visp3/core/vpRobust.h>
43#include <visp3/vision/vpPose.h>
55 double residu_1 = 1e8;
62 unsigned int nb = (
unsigned int)
listP.size();
69 std::list<vpPoint> lP;
73 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
75 sd[2 * k] = P.
get_x();
76 sd[2 * k + 1] = P.
get_y();
85 while (std::fabs(residu_1 - r) > vvsEpsilon) {
90 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
97 double x = s[2 * k] = P.
get_x();
98 double y = s[2 * k + 1] = P.
get_y();
100 L[2 * k][0] = -1 / Z;
104 L[2 * k][4] = -(1 + x * x);
108 L[2 * k + 1][1] = -1 / Z;
109 L[2 * k + 1][2] = y / Z;
110 L[2 * k + 1][3] = 1 + y * y;
111 L[2 * k + 1][4] = -x * y;
112 L[2 * k + 1][5] = -x;
123 L.pseudoInverse(Lp, 1e-16);
134 if (iter++ > vvsIterMax) {
139 if (computeCovariance)
160 double residu_1 = 1e8;
169 unsigned int nb = (
unsigned int)
listP.
size();
176 std::list<vpPoint> lP;
180 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
182 sd[2 * k_] = P.
get_x();
183 sd[2 * k_ + 1] = P.
get_y();
194 while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
199 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
206 double x = s[2 * k_] = P.
get_x();
207 double y = s[2 * k_ + 1] = P.
get_y();
208 double Z = P.
get_Z();
209 L[2 * k_][0] = -1 / Z;
211 L[2 * k_][2] = x / Z;
212 L[2 * k_][3] = x * y;
213 L[2 * k_][4] = -(1 + x * x);
216 L[2 * k_ + 1][0] = 0;
217 L[2 * k_ + 1][1] = -1 / Z;
218 L[2 * k_ + 1][2] = y / Z;
219 L[2 * k_ + 1][3] = 1 + y * y;
220 L[2 * k_ + 1][4] = -x * y;
221 L[2 * k_ + 1][5] = -x;
230 for (
unsigned int k = 0; k < error.
getRows() / 2; k++) {
236 for (
unsigned int k = 0; k < error.
getRows() / 2; k++) {
237 W[2 * k][2 * k] = w[k];
238 W[2 * k + 1][2 * k + 1] = w[k];
242 (W * L).pseudoInverse(Lp, 1e-6);
245 v = -
lambda * Lp * W * error;
248 if (iter++ > vvsIterMax)
252 if (computeCovariance)
263#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
264 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
276 auto residu_1{1e8}, r{1e8 - 1};
277 const auto lambda{0.9}, vvsEpsilon{1e-8};
278 const unsigned int vvsIterMax{200};
280 const unsigned int nb =
static_cast<unsigned int>(points.size());
286 for (
auto i = 0u; i < points.size(); i++) {
287 sd[3 * i] = points[i].get_x();
288 sd[3 * i + 1] = points[i].get_y();
289 sd[3 * i + 2] = points[i].get_Z();
294 while (std::fabs(residu_1 - r) > vvsEpsilon) {
298 for (
auto i = 0u; i < points.size(); i++) {
303 points.at(i).changeFrame(cMo, cP);
304 points.at(i).projection(cP, p);
306 const auto x = s[3 * i] = p[0];
307 const auto y = s[3 * i + 1] = p[1];
308 const auto Z = s[3 * i + 2] = cP[2];
309 L[3 * i][0] = -1 / Z;
317 L[3 * i + 1][1] = -1 / Z;
318 L[3 * i + 1][2] = y / Z;
320 L[3 * i + 1][4] = -x * y;
321 L[3 * i + 1][5] = -x;
325 L[3 * i + 2][2] = -1;
326 L[3 * i + 2][3] = -y * Z;
327 L[3 * i + 2][4] = x * Z;
328 L[3 * i + 2][5] = -0;
337 L.pseudoInverse(Lp, 1e-16);
340 const auto v = -
lambda * Lp * err;
345 if (iter++ > vvsIterMax) {
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int size() const
Return the number of elements of the 2D array.
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)