Visual Servoing Platform  version 3.3.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
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  try {
64  int sum = 0;
65  for (double i = -0.2; i < 0.2; i += 0.1) {
66  for (double j = -0.2; j < 0.2; j += 0.1) {
67  for (double k = -vpMath::rad(30); k < vpMath::rad(30); k += vpMath::rad(10)) {
68  for (double l = 0.5; l < 1.5; l += 0.1) {
69  sum += test(i, j, l, k);
70  }
71  }
72  }
73  }
74  if (sum < 0)
75  return -1;
76  else
77  return 0;
78  } catch (const vpException &e) {
79  std::cout << "Catch an exception: " << e << std::endl;
80  return 1;
81  }
82 }
83 
84 int test(double x, double y, double z, double alpha)
85 {
86  // intial pose
87  vpHomogeneousMatrix cMo(x, y, z, -vpMath::rad(0), vpMath::rad(0), alpha);
88  // Desired pose
90 
91  // source and destination objects for moment manipulation
92  vpMomentObject src(6);
93  vpMomentObject dst(6);
94 
95  // init and run the simulation
96  initScene(cMo, cdMo, src, dst); // initialize graphical scene (for
97  // interface)
98 
99  vpMatrix mat = execute(cMo, cdMo, src, dst);
100 
101  if (fabs(mat[0][0] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
102  return -1;
103  if (fabs(mat[0][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
104  return -1;
105  if (fabs(mat[0][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
106  return -1;
107 
108  if (fabs(mat[1][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
109  return -1;
110  if (fabs(mat[1][1] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
111  return -1;
112  if (fabs(mat[1][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
113  return -1;
114 
115  if (fabs(mat[2][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
116  return -1;
117  if (fabs(mat[2][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
118  return -1;
119  if (fabs(mat[2][2] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
120  return -1;
121  if (fabs(mat[2][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
122  return -1;
123 
124  if (fabs(mat[3][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
125  return -1;
126  if (fabs(mat[3][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
127  return -1;
128  if (fabs(mat[3][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
129  return -1;
130  if (fabs(mat[3][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
131  return -1;
132 
133  if (fabs(mat[4][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
134  return -1;
135  if (fabs(mat[4][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
136  return -1;
137  if (fabs(mat[4][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
138  return -1;
139  if (fabs(mat[4][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
140  return -1;
141 
142  if (fabs(mat[5][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
143  return -1;
144  if (fabs(mat[5][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
145  return -1;
146  if (fabs(mat[5][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
147  return -1;
148  if (fabs(mat[5][5] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
149  return -1;
150 
151  return 0;
152 }
153 
154 void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
155  vpMomentObject &dst)
156 {
157  std::vector<vpPoint> src_pts;
158  std::vector<vpPoint> dst_pts;
159 
160  double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
161  double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
162  int nbpoints = 4;
163 
164  for (int i = 0; i < nbpoints; i++) {
165  vpPoint p(x[i], y[i], 0.0);
166  p.track(cMo);
167  src_pts.push_back(p);
168  }
169 
171  src.fromVector(src_pts);
172  for (int i = 0; i < nbpoints; i++) {
173  vpPoint p(x[i], y[i], 0.0);
174  p.track(cdMo);
175  dst_pts.push_back(p);
176  }
178  dst.fromVector(dst_pts);
179 }
180 
181 vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
182  vpMomentObject &dst)
183 {
185  ; // current or desired
186 
187  vpServo task;
189  // A,B,C parameters of source and destination plane
190  double A;
191  double B;
192  double C;
193  double Ad;
194  double Bd;
195  double Cd;
196  // init main object: using moments up to order 6
197 
198  // Initializing values from regular plane (with ax+by+cz=d convention)
199  vpPlane pl;
200  pl.setABCD(0, 0, 1.0, 0);
201  pl.changeFrame(cMo);
202  planeToABC(pl, A, B, C);
203 
204  pl.setABCD(0, 0, 1.0, 0);
205  pl.changeFrame(cdMo);
206  planeToABC(pl, Ad, Bd, Cd);
207 
208  // extracting initial position (actually we only care about Zdst)
210  cdMo.extract(vec);
211 
214  // don't need to be specific, vpMomentCommon automatically loads
215  // Xg,Yg,An,Ci,Cj,Alpha moments
217  vec[2]);
219  vec[2]);
220  // same thing with common features
221  vpFeatureMomentCommon featureMoments(moments);
222  vpFeatureMomentCommon featureMomentsDes(momentsDes);
223 
224  moments.updateAll(src);
225  momentsDes.updateAll(dst);
226 
227  featureMoments.updateAll(A, B, C);
228  featureMomentsDes.updateAll(Ad, Bd, Cd);
229 
230  // setup the interaction type
231  task.setInteractionMatrixType(interaction_type);
234  task.addFeature(featureMoments.getFeatureGravityNormalized(), featureMomentsDes.getFeatureGravityNormalized());
235  task.addFeature(featureMoments.getFeatureAn(), featureMomentsDes.getFeatureAn());
236  // the moments are different in case of a symmetric object
237  task.addFeature(featureMoments.getFeatureCInvariant(), featureMomentsDes.getFeatureCInvariant(),
238  (1 << 10) | (1 << 11));
239  task.addFeature(featureMoments.getFeatureAlpha(), featureMomentsDes.getFeatureAlpha());
240 
241  task.setLambda(0.4);
242 
243  task.computeControlLaw();
244  vpMatrix mat = task.computeInteractionMatrix();
245  task.kill();
246  return mat;
247 }
248 
249 void planeToABC(const vpPlane &pl, double &A, double &B, double &C)
250 {
251  A = -pl.getA() / pl.getD();
252  B = -pl.getB() / pl.getD();
253  C = -pl.getC() / pl.getD();
254 }
vpFeatureMomentCommon
This class allows to access common vpFeatureMoments in a pre-filled database.
Definition: vpFeatureMomentCommon.h:225
vpServo::kill
void kill()
Definition: vpServo.cpp:191
vpMomentObject::setType
void setType(vpObjectType input_type)
Definition: vpMomentObject.h:293
vpServo::CURRENT
Definition: vpServo.h:185
vpMath::rad
static double rad(double deg)
Definition: vpMath.h:107
vpMomentCommon::updateAll
void updateAll(vpMomentObject &object)
Definition: vpMomentCommon.cpp:125
vpFeatureMomentCommon::getFeatureAlpha
vpFeatureMomentAlpha & getFeatureAlpha()
Definition: vpFeatureMomentCommon.h:243
vpMomentCommon
This class initializes and allows access to commonly used moments.
Definition: vpMomentCommon.h:103
vpServo::setLambda
void setLambda(double c)
Definition: vpServo.h:405
vpServo::EYEINHAND_CAMERA
Definition: vpServo.h:158
vpServo::computeInteractionMatrix
vpMatrix computeInteractionMatrix()
Definition: vpServo.cpp:653
vpTranslationVector
Class that consider the case of a translation vector.
Definition: vpTranslationVector.h:118
vpMomentObject::DENSE_POLYGON
Definition: vpMomentObject.h:227
vpHomogeneousMatrix::extract
void extract(vpRotationMatrix &R) const
Definition: vpHomogeneousMatrix.cpp:550
vpMatrix
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:163
vpServo::setServo
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:222
vpPlane::setABCD
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:89
vpFeatureMomentCommon::getFeatureGravityNormalized
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
Definition: vpFeatureMomentCommon.h:265
vpMomentObject::fromVector
void fromVector(std::vector< vpPoint > &points)
Definition: vpMomentObject.cpp:229
vpPlane::getB
double getB() const
Definition: vpPlane.h:103
vpFeatureMomentCommon::updateAll
void updateAll(double A, double B, double C)
Definition: vpFeatureMomentCommon.cpp:73
vpPlane::getC
double getC() const
Definition: vpPlane.h:105
vpMomentCommon::getMu3
static std::vector< double > getMu3(vpMomentObject &object)
Definition: vpMomentCommon.cpp:198
vpServo::addFeature
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:496
vpMomentObject
Class for generic objects.
Definition: vpMomentObject.h:218
vpPlane::getD
double getD() const
Definition: vpPlane.h:107
vpPlane
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:57
vpPlane::changeFrame
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:353
vpServo::setInteractionMatrixType
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:573
vpServo
Definition: vpServo.h:149
vpServo::vpServoIteractionMatrixType
vpServoIteractionMatrixType
Definition: vpServo.h:184
vpServo::computeControlLaw
vpColVector computeControlLaw()
Definition: vpServo.cpp:934
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:57
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:148
vpMomentCommon::getSurface
static double getSurface(vpMomentObject &object)
Definition: vpMomentCommon.cpp:148
vpFeatureMomentCommon::getFeatureCInvariant
vpFeatureMomentCInvariant & getFeatureCInvariant()
Definition: vpFeatureMomentCommon.h:261
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpMomentCommon::getAlpha
static double getAlpha(vpMomentObject &object)
Definition: vpMomentCommon.cpp:175
vpFeatureMomentCommon::getFeatureAn
vpFeatureMomentAreaNormalized & getFeatureAn()
Definition: vpFeatureMomentCommon.h:248
vpPlane::getA
double getA() const
Definition: vpPlane.h:101