Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testFeatureMoment.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See https://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Example of visual servoing with moments using a polygon as object container
33 *
34*****************************************************************************/
35#include <visp3/core/vpDebug.h>
36#include <visp3/core/vpHomogeneousMatrix.h>
37#include <visp3/core/vpMomentCommon.h>
38#include <visp3/core/vpMomentDatabase.h>
39#include <visp3/core/vpMomentObject.h>
40#include <visp3/core/vpPlane.h>
41#include <visp3/visual_features/vpFeatureMomentCommon.h>
42#include <visp3/vs/vpServo.h>
43
44#include <iostream>
45#include <limits>
46
47// initialize scene in the interface
48void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
49 vpMomentObject &dst);
50
51vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
52 vpMomentObject &dst); // launch the test
53void planeToABC(const vpPlane &pl, double &A, double &B, double &C);
54int test(double x, double y, double z, double alpha);
55
56// Compute a set of parallel positions and check if the matrix is in the right
57// form;
58int main()
59{
60#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
61 try {
62 int sum = 0;
63 for (double i = -0.2; i < 0.2; i += 0.1) {
64 for (double j = -0.2; j < 0.2; j += 0.1) {
65 for (double k = -vpMath::rad(30); k < vpMath::rad(30); k += vpMath::rad(10)) {
66 for (double l = 0.5; l < 1.5; l += 0.1) {
67 sum += test(i, j, l, k);
68 }
69 }
70 }
71 }
72 if (sum < 0)
73 return EXIT_FAILURE;
74 else
75 return EXIT_SUCCESS;
76 }
77 catch (const vpException &e) {
78 std::cout << "Catch an exception: " << e << std::endl;
79 return EXIT_FAILURE;
80 }
81#else
82 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
83 return EXIT_SUCCESS;
84#endif
85}
86
87int test(double x, double y, double z, double alpha)
88{
89 // intial pose
90 vpHomogeneousMatrix cMo(x, y, z, -vpMath::rad(0), vpMath::rad(0), alpha);
91 // Desired pose
93
94 // source and destination objects for moment manipulation
95 vpMomentObject src(6);
96 vpMomentObject dst(6);
97
98 // init and run the simulation
99 initScene(cMo, cdMo, src, dst); // initialize graphical scene (for
100 // interface)
101
102 vpMatrix mat = execute(cMo, cdMo, src, dst);
103
104 if (fabs(mat[0][0] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
105 return -1;
106 if (fabs(mat[0][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
107 return -1;
108 if (fabs(mat[0][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
109 return -1;
110
111 if (fabs(mat[1][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
112 return -1;
113 if (fabs(mat[1][1] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
114 return -1;
115 if (fabs(mat[1][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
116 return -1;
117
118 if (fabs(mat[2][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
119 return -1;
120 if (fabs(mat[2][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
121 return -1;
122 if (fabs(mat[2][2] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
123 return -1;
124 if (fabs(mat[2][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
125 return -1;
126
127 if (fabs(mat[3][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
128 return -1;
129 if (fabs(mat[3][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
130 return -1;
131 if (fabs(mat[3][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
132 return -1;
133 if (fabs(mat[3][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
134 return -1;
135
136 if (fabs(mat[4][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
137 return -1;
138 if (fabs(mat[4][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
139 return -1;
140 if (fabs(mat[4][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
141 return -1;
142 if (fabs(mat[4][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
143 return -1;
144
145 if (fabs(mat[5][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
146 return -1;
147 if (fabs(mat[5][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
148 return -1;
149 if (fabs(mat[5][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
150 return -1;
151 if (fabs(mat[5][5] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
152 return -1;
153
154 return 0;
155}
156
157void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
158 vpMomentObject &dst)
159{
160 std::vector<vpPoint> src_pts;
161 std::vector<vpPoint> dst_pts;
162
163 double x[5] = { 0.2, 0.2, -0.2, -0.2, 0.2 };
164 double y[5] = { -0.1, 0.1, 0.1, -0.1, -0.1 };
165 int nbpoints = 4;
166
167 for (int i = 0; i < nbpoints; i++) {
168 vpPoint p(x[i], y[i], 0.0);
169 p.track(cMo);
170 src_pts.push_back(p);
171 }
172
174 src.fromVector(src_pts);
175 for (int i = 0; i < nbpoints; i++) {
176 vpPoint p(x[i], y[i], 0.0);
177 p.track(cdMo);
178 dst_pts.push_back(p);
179 }
181 dst.fromVector(dst_pts);
182}
183
184vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
185 vpMomentObject &dst)
186{
187 vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT; // current or desired
188
189 vpServo task;
191 // A,B,C parameters of source and destination plane
192 double A;
193 double B;
194 double C;
195 double Ad;
196 double Bd;
197 double Cd;
198 // init main object: using moments up to order 6
199
200 // Initializing values from regular plane (with ax+by+cz=d convention)
201 vpPlane pl;
202 pl.setABCD(0, 0, 1.0, 0);
203 pl.changeFrame(cMo);
204 planeToABC(pl, A, B, C);
205
206 pl.setABCD(0, 0, 1.0, 0);
207 pl.changeFrame(cdMo);
208 planeToABC(pl, Ad, Bd, Cd);
209
210 // extracting initial position (actually we only care about Zdst)
212 cdMo.extract(vec);
213
216 // don't need to be specific, vpMomentCommon automatically loads
217 // Xg,Yg,An,Ci,Cj,Alpha moments
219 vec[2]);
221 vec[2]);
222 // same thing with common features
223 vpFeatureMomentCommon featureMoments(moments);
224 vpFeatureMomentCommon featureMomentsDes(momentsDes);
225
226 moments.updateAll(src);
227 momentsDes.updateAll(dst);
228
229 featureMoments.updateAll(A, B, C);
230 featureMomentsDes.updateAll(Ad, Bd, Cd);
231
232 // setup the interaction type
233 task.setInteractionMatrixType(interaction_type);
236 task.addFeature(featureMoments.getFeatureGravityNormalized(), featureMomentsDes.getFeatureGravityNormalized());
237 task.addFeature(featureMoments.getFeatureAn(), featureMomentsDes.getFeatureAn());
238 // the moments are different in case of a symmetric object
239 task.addFeature(featureMoments.getFeatureCInvariant(), featureMomentsDes.getFeatureCInvariant(),
240 (1 << 10) | (1 << 11));
241 task.addFeature(featureMoments.getFeatureAlpha(), featureMomentsDes.getFeatureAlpha());
242
243 task.setLambda(0.4);
244
245 task.computeControlLaw();
247 return mat;
248}
249
250void planeToABC(const vpPlane &pl, double &A, double &B, double &C)
251{
252 A = -pl.getA() / pl.getD();
253 B = -pl.getB() / pl.getD();
254 C = -pl.getC() / pl.getD();
255}
error that can be emitted by ViSP classes.
Definition vpException.h:59
This class allows to access common vpFeatureMoments in a pre-filled database.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static double rad(double deg)
Definition vpMath.h:116
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
This class initializes and allows access to commonly used moments.
static std::vector< double > getMu3(vpMomentObject &object)
static double getAlpha(vpMomentObject &object)
static double getSurface(vpMomentObject &object)
Class for generic objects.
void setType(vpObjectType input_type)
void fromVector(std::vector< vpPoint > &points)
This class defines the container for a plane geometrical structure.
Definition vpPlane.h:54
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition vpPlane.cpp:361
double getD() const
Definition vpPlane.h:106
double getA() const
Definition vpPlane.h:100
double getC() const
Definition vpPlane.h:104
void setABCD(double a, double b, double c, double d)
Definition vpPlane.h:88
double getB() const
Definition vpPlane.h:102
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition vpServo.cpp:564
@ EYEINHAND_CAMERA
Definition vpServo.h:151
void setLambda(double c)
Definition vpServo.h:403
void setServo(const vpServoType &servo_type)
Definition vpServo.cpp:210
vpMatrix computeInteractionMatrix()
Definition vpServo.cpp:644
vpColVector computeControlLaw()
Definition vpServo.cpp:930
vpServoIteractionMatrixType
Definition vpServo.h:178
@ CURRENT
Definition vpServo.h:179
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition vpServo.cpp:487
Class that consider the case of a translation vector.