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 = (
unsigned int)
listP.size();
63 std::list<vpPoint> lP;
67 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
69 sd[2 * k] = P.
get_x();
70 sd[2 * k + 1] = P.
get_y();
79 while (std::fabs(residu_1 - r) > vvsEpsilon) {
84 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
91 double x = s[2 * k] = P.
get_x();
92 double y = s[2 * k + 1] = P.
get_y();
98 L[2 * k][4] = -(1 + x * x);
102 L[2 * k + 1][1] = -1 / Z;
103 L[2 * k + 1][2] = y / Z;
104 L[2 * k + 1][3] = 1 + y * y;
105 L[2 * k + 1][4] = -x * y;
106 L[2 * k + 1][5] = -x;
117 L.pseudoInverse(Lp, 1e-16);
128 if (iter++ > vvsIterMax) {
133 if (computeCovariance)
147 double residu_1 = 1e8;
156 unsigned int nb = (
unsigned int)
listP.size();
163 std::list<vpPoint> lP;
167 for (std::list<vpPoint>::const_iterator it =
listP.begin(); it !=
listP.end(); ++it) {
169 sd[2 * k_] = P.
get_x();
170 sd[2 * k_ + 1] = P.
get_y();
181 while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
186 for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
193 double x = s[2 * k_] = P.
get_x();
194 double y = s[2 * k_ + 1] = P.
get_y();
195 double Z = P.
get_Z();
196 L[2 * k_][0] = -1 / Z;
198 L[2 * k_][2] = x / Z;
199 L[2 * k_][3] = x * y;
200 L[2 * k_][4] = -(1 + x * x);
203 L[2 * k_ + 1][0] = 0;
204 L[2 * k_ + 1][1] = -1 / Z;
205 L[2 * k_ + 1][2] = y / Z;
206 L[2 * k_ + 1][3] = 1 + y * y;
207 L[2 * k_ + 1][4] = -x * y;
208 L[2 * k_ + 1][5] = -x;
217 for (
unsigned int k = 0; k < error.
getRows() / 2; k++) {
223 for (
unsigned int k = 0; k < error.
getRows() / 2; k++) {
224 W[2 * k][2 * k] = w[k];
225 W[2 * k + 1][2 * k + 1] = w[k];
229 (W * L).pseudoInverse(Lp, 1e-6);
235 if (iter++ > vvsIterMax)
239 if (computeCovariance)
252 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
253 std::optional<vpHomogeneousMatrix> vpPose::poseVirtualVSWithDepth(
const std::vector<vpPoint> &points,
const vpHomogeneousMatrix &cMo)
255 auto residu_1 { 1e8 }, r { 1e8 - 1 };
256 const auto lambda { 0.9 }, vvsEpsilon { 1e-8 };
257 const unsigned int vvsIterMax { 200 };
259 const unsigned int nb =
static_cast<unsigned int>(points.size());
265 for (
auto i = 0u; i < points.size(); i++) {
266 sd[3 * i] = points[i].get_x();
267 sd[3 * i + 1] = points[i].get_y();
268 sd[3 * i + 2] = points[i].get_Z();
273 while (std::fabs(residu_1 - r) > vvsEpsilon) {
277 for (
auto i = 0u; i < points.size(); i++) {
282 points.at(i).changeFrame(cMo, cP);
283 points.at(i).projection(cP, p);
285 const auto x = s[3 * i] = p[0];
286 const auto y = s[3 * i + 1] = p[1];
287 const auto Z = s[3 * i + 2] = cP[2];
288 L[3 * i][0] = -1 / Z;
296 L[3 * i + 1][1] = -1 / Z;
297 L[3 * i + 1][2] = y / Z;
299 L[3 * i + 1][4] = -x * y;
300 L[3 * i + 1][5] = -x;
304 L[3 * i + 2][2] = -1;
305 L[3 * i + 2][3] = -y * Z;
306 L[3 * i + 2][4] = x * Z;
307 L[3 * i + 2][5] = -0;
316 L.pseudoInverse(Lp, 1e-16);
319 const auto v = -lambda * Lp * err;
324 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.
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)