Visual Servoing Platform version 3.5.0
testFeatureMoment.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * Authors:
35 * Filip Novotny
36 *
37 *****************************************************************************/
38#include <visp3/core/vpDebug.h>
39#include <visp3/core/vpHomogeneousMatrix.h>
40#include <visp3/core/vpMomentCommon.h>
41#include <visp3/core/vpMomentDatabase.h>
42#include <visp3/core/vpMomentObject.h>
43#include <visp3/core/vpPlane.h>
44#include <visp3/visual_features/vpFeatureMomentCommon.h>
45#include <visp3/vs/vpServo.h>
46
47#include <iostream>
48#include <limits>
49
50// initialize scene in the interface
51void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
52 vpMomentObject &dst);
53
54vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
55 vpMomentObject &dst); // launch the test
56void planeToABC(const vpPlane &pl, double &A, double &B, double &C);
57int test(double x, double y, double z, double alpha);
58
59// Compute a set of parallel positions and check if the matrix is in the right
60// form;
61int main()
62{
63#if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
64 try {
65 int sum = 0;
66 for (double i = -0.2; i < 0.2; i += 0.1) {
67 for (double j = -0.2; j < 0.2; j += 0.1) {
68 for (double k = -vpMath::rad(30); k < vpMath::rad(30); k += vpMath::rad(10)) {
69 for (double l = 0.5; l < 1.5; l += 0.1) {
70 sum += test(i, j, l, k);
71 }
72 }
73 }
74 }
75 if (sum < 0)
76 return EXIT_FAILURE;
77 else
78 return EXIT_SUCCESS;
79 } catch (const vpException &e) {
80 std::cout << "Catch an exception: " << e << std::endl;
81 return EXIT_FAILURE;
82 }
83#else
84 std::cout << "Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
85 return EXIT_SUCCESS;
86#endif
87}
88
89int test(double x, double y, double z, double alpha)
90{
91 // intial pose
92 vpHomogeneousMatrix cMo(x, y, z, -vpMath::rad(0), vpMath::rad(0), alpha);
93 // Desired pose
95
96 // source and destination objects for moment manipulation
97 vpMomentObject src(6);
98 vpMomentObject dst(6);
99
100 // init and run the simulation
101 initScene(cMo, cdMo, src, dst); // initialize graphical scene (for
102 // interface)
103
104 vpMatrix mat = execute(cMo, cdMo, src, dst);
105
106 if (fabs(mat[0][0] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
107 return -1;
108 if (fabs(mat[0][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
109 return -1;
110 if (fabs(mat[0][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
111 return -1;
112
113 if (fabs(mat[1][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
114 return -1;
115 if (fabs(mat[1][1] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
116 return -1;
117 if (fabs(mat[1][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
118 return -1;
119
120 if (fabs(mat[2][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
121 return -1;
122 if (fabs(mat[2][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
123 return -1;
124 if (fabs(mat[2][2] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
125 return -1;
126 if (fabs(mat[2][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
127 return -1;
128
129 if (fabs(mat[3][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
130 return -1;
131 if (fabs(mat[3][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
132 return -1;
133 if (fabs(mat[3][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
134 return -1;
135 if (fabs(mat[3][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
136 return -1;
137
138 if (fabs(mat[4][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
139 return -1;
140 if (fabs(mat[4][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
141 return -1;
142 if (fabs(mat[4][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
143 return -1;
144 if (fabs(mat[4][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
145 return -1;
146
147 if (fabs(mat[5][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
148 return -1;
149 if (fabs(mat[5][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
150 return -1;
151 if (fabs(mat[5][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
152 return -1;
153 if (fabs(mat[5][5] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
154 return -1;
155
156 return 0;
157}
158
159void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
160 vpMomentObject &dst)
161{
162 std::vector<vpPoint> src_pts;
163 std::vector<vpPoint> dst_pts;
164
165 double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
166 double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
167 int nbpoints = 4;
168
169 for (int i = 0; i < nbpoints; i++) {
170 vpPoint p(x[i], y[i], 0.0);
171 p.track(cMo);
172 src_pts.push_back(p);
173 }
174
176 src.fromVector(src_pts);
177 for (int i = 0; i < nbpoints; i++) {
178 vpPoint p(x[i], y[i], 0.0);
179 p.track(cdMo);
180 dst_pts.push_back(p);
181 }
183 dst.fromVector(dst_pts);
184}
185
186vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
187 vpMomentObject &dst)
188{
190 ; // current or desired
191
192 vpServo task;
194 // A,B,C parameters of source and destination plane
195 double A;
196 double B;
197 double C;
198 double Ad;
199 double Bd;
200 double Cd;
201 // init main object: using moments up to order 6
202
203 // Initializing values from regular plane (with ax+by+cz=d convention)
204 vpPlane pl;
205 pl.setABCD(0, 0, 1.0, 0);
206 pl.changeFrame(cMo);
207 planeToABC(pl, A, B, C);
208
209 pl.setABCD(0, 0, 1.0, 0);
210 pl.changeFrame(cdMo);
211 planeToABC(pl, Ad, Bd, Cd);
212
213 // extracting initial position (actually we only care about Zdst)
215 cdMo.extract(vec);
216
219 // don't need to be specific, vpMomentCommon automatically loads
220 // Xg,Yg,An,Ci,Cj,Alpha moments
222 vec[2]);
224 vec[2]);
225 // same thing with common features
226 vpFeatureMomentCommon featureMoments(moments);
227 vpFeatureMomentCommon featureMomentsDes(momentsDes);
228
229 moments.updateAll(src);
230 momentsDes.updateAll(dst);
231
232 featureMoments.updateAll(A, B, C);
233 featureMomentsDes.updateAll(Ad, Bd, Cd);
234
235 // setup the interaction type
236 task.setInteractionMatrixType(interaction_type);
239 task.addFeature(featureMoments.getFeatureGravityNormalized(), featureMomentsDes.getFeatureGravityNormalized());
240 task.addFeature(featureMoments.getFeatureAn(), featureMomentsDes.getFeatureAn());
241 // the moments are different in case of a symmetric object
242 task.addFeature(featureMoments.getFeatureCInvariant(), featureMomentsDes.getFeatureCInvariant(),
243 (1 << 10) | (1 << 11));
244 task.addFeature(featureMoments.getFeatureAlpha(), featureMomentsDes.getFeatureAlpha());
245
246 task.setLambda(0.4);
247
248 task.computeControlLaw();
250 return mat;
251}
252
253void planeToABC(const vpPlane &pl, double &A, double &B, double &C)
254{
255 A = -pl.getA() / pl.getD();
256 B = -pl.getB() / pl.getD();
257 C = -pl.getC() / pl.getD();
258}
error that can be emited by ViSP classes.
Definition: vpException.h:72
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:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
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:59
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
double getD() const
Definition: vpPlane.h:108
double getA() const
Definition: vpPlane.h:102
double getC() const
Definition: vpPlane.h:106
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:90
double getB() const
Definition: vpPlane.h:104
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:82
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
void setLambda(double c)
Definition: vpServo.h:404
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:647
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
vpServoIteractionMatrixType
Definition: vpServo.h:181
@ CURRENT
Definition: vpServo.h:182
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
Class that consider the case of a translation vector.