Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
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
51 void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
52  vpMomentObject &dst);
53 
54 vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
55  vpMomentObject &dst); // launch the test
56 void planeToABC(const vpPlane &pl, double &A, double &B, double &C);
57 int 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;
61 int 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 
89 int 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 
159 void 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 
186 vpMatrix 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();
249  vpMatrix mat = task.computeInteractionMatrix();
250  return mat;
251 }
252 
253 void 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 }
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
Implementation of an homogeneous matrix and operations on such kind of matrices.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:490
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
Definition: vpException.h:71
Class for generic objects.
void extract(vpRotationMatrix &R) const
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:81
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:647
double getD() const
Definition: vpPlane.h:108
vpColVector computeControlLaw()
Definition: vpServo.cpp:929
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
static std::vector< double > getMu3(vpMomentObject &object)
double getB() const
Definition: vpPlane.h:104
void setLambda(double c)
Definition: vpServo.h:404
vpServoIteractionMatrixType
Definition: vpServo.h:181
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:90
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:567
static double rad(double deg)
Definition: vpMath.h:110
double getA() const
Definition: vpPlane.h:102
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
void setType(vpObjectType input_type)
double getC() const
Definition: vpPlane.h:106
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:218
Class that consider the case of a translation vector.