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>
49 double residu_1 = 1e8;
56 unsigned int nb =
static_cast<unsigned int>(
listP.size());
63 std::list<vpPoint> lP;
67 std::list<vpPoint>::const_iterator listp_end =
listP.end();
68 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
70 sd[2 * k] = P.
get_x();
71 sd[(2 * k) + 1] = P.
get_y();
80 while (std::fabs(residu_1 - r) > vvsEpsilon) {
85 std::list<vpPoint>::const_iterator lp_end = lP.end();
86 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lp_end; ++it) {
93 double x = s[2 * k] = P.
get_x();
94 double y = s[(2 * k) + 1] = P.
get_y();
100 L[2 * k][4] = -(1 + (x * x));
103 L[(2 * k) + 1][0] = 0;
104 L[(2 * k) + 1][1] = -1 / Z;
105 L[(2 * k) + 1][2] = y / Z;
106 L[(2 * k) + 1][3] = 1 + (y * y);
107 L[(2 * k) + 1][4] = -x * y;
108 L[(2 * k) + 1][5] = -x;
119 L.pseudoInverse(Lp, 1e-16);
129 if (iter> vvsIterMax) {
137 if (computeCovariance) {
152 double residu_1 = 1e8;
161 unsigned int nb =
static_cast<unsigned int>(
listP.size());
168 std::list<vpPoint> lP;
172 std::list<vpPoint>::const_iterator listp_end =
listP.end();
173 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it != listp_end; ++it) {
175 sd[2 * k_] = P.
get_x();
176 sd[(2 * k_) + 1] = P.
get_y();
187 while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
192 std::list<vpPoint>::const_iterator lp_end = lP.end();
193 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lp_end; ++it) {
200 double x = s[2 * k_] = P.
get_x();
201 double y = s[(2 * k_) + 1] = P.
get_y();
202 double Z = P.
get_Z();
203 L[2 * k_][0] = -1 / Z;
205 L[2 * k_][2] = x / Z;
206 L[2 * k_][3] = x * y;
207 L[2 * k_][4] = -(1 + (x * x));
210 L[(2 * k_) + 1][0] = 0;
211 L[(2 * k_) + 1][1] = -1 / Z;
212 L[(2 * k_) + 1][2] = y / Z;
213 L[(2 * k_) + 1][3] = 1 + (y * y);
214 L[(2 * k_) + 1][4] = -x * y;
215 L[(2 * k_) + 1][5] = -x;
224 unsigned int v_error_rows = error.
getRows();
225 for (
unsigned int k = 0; k < (v_error_rows / 2); ++k) {
231 unsigned int error_rows = error.
getRows();
232 for (
unsigned int k = 0; k < (error_rows / 2); ++k) {
233 W[2 * k][2 * k] = w[k];
234 W[(2 * k) + 1][(2 * k) + 1] = w[k];
238 (W * L).pseudoInverse(Lp, 1e-6);
244 if (iter > vvsIterMax) {
252 if (computeCovariance) {
266 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
267 std::optional<vpHomogeneousMatrix> vpPose::poseVirtualVSWithDepth(
const std::vector<vpPoint> &points,
const vpHomogeneousMatrix &cMo)
269 auto residu_1 { 1e8 }, r { 1e8 - 1 };
270 const auto lambda { 0.9 }, vvsEpsilon { 1e-8 };
271 const unsigned int vvsIterMax { 200 };
273 const unsigned int nb =
static_cast<unsigned int>(points.size());
279 auto v_points_size = points.size();
280 for (
auto i = 0u; i < v_points_size; ++i) {
281 sd[3 * i] = points[i].get_x();
282 sd[(3 * i) + 1] = points[i].get_y();
283 sd[(3 * i) + 2] = points[i].get_Z();
288 while (std::fabs(residu_1 - r) > vvsEpsilon) {
292 auto points_size = points.
size();
293 for (
auto i = 0u; i < points_size; ++i) {
298 points.at(i).changeFrame(cMo, cP);
299 points.at(i).projection(cP, p);
301 const auto x = s[3 * i] = p[0];
302 const auto y = s[(3 * i) + 1] = p[1];
303 const auto Z = s[(3 * i) + 2] = cP[2];
304 L[3 * i][0] = -1 / Z;
311 L[(3 * i) + 1][0] = 0;
312 L[(3 * i) + 1][1] = -1 / Z;
313 L[(3 * i) + 1][2] = y / Z;
315 L[(3 * i) + 1][4] = -x * y;
316 L[(3 * i) + 1][5] = -x;
318 L[(3 * i) + 2][0] = 0;
319 L[(3 * i) + 2][1] = 0;
320 L[(3 * i) + 2][2] = -1;
321 L[(3 * i) + 2][3] = -y * Z;
322 L[(3 * i) + 2][4] = x * Z;
323 L[(3 * i) + 2][5] = -0;
332 L.pseudoInverse(Lp, 1e-16);
335 const auto v = -lambda * Lp * err;
340 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.
double m_lambda
Parameters use for the virtual visual servoing approach.
void poseVirtualVS(vpHomogeneousMatrix &cMo)
std::list< vpPoint > listP
Array of point (use here class vpPoint)
void poseVirtualVSrobust(vpHomogeneousMatrix &cMo)
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)