ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
testFeatureMoment.cpp
1 /****************************************************************************
2  *
3  * $Id: testFeatureMoment.cpp 3323 2011-09-13 15:23:56Z fnovotny $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Example of visual servoing with moments using a polygon as object container
36  *
37  * Authors:
38  * Filip Novotny
39  *
40  *****************************************************************************/
41 #include <visp/vpMomentObject.h>
42 #include <visp/vpMomentDatabase.h>
43 #include <visp/vpMomentCommon.h>
44 #include <visp/vpFeatureMomentCommon.h>
45 #include <visp/vpHomogeneousMatrix.h>
46 #include <visp/vpServo.h>
47 #include <visp/vpDebug.h>
48 #include <visp/vpPlane.h>
49 
50 #include <limits>
51 #include <iostream>
52 
53 //initialize scene in the interface
54 void initScene();
55 
56 void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
57 vpMatrix execute(); //launch the test
58 void _planeToABC(vpPlane& pl, double& A,double& B, double& C);
59 int test(double x,double y,double z,double alpha);
60 
61 //start and destination positioning matrices
64 
65 vpServo::vpServoIteractionMatrixType interaction_type; //current or desired
66 
67 //source and destination objects for moment manipulation
68 vpMomentObject src(6);
69 vpMomentObject dst(6);
70 
71 using namespace std;
72 
73 //Compute a set of parallel positions and check if the matrix is in the right form;
74 int main()
75 {
76  int sum=0;
77  for(double i=-0.2;i<0.2;i+=0.1){
78  for(double j=-0.2;j<0.2;j+=0.1){
79  for(double k=-vpMath::rad(30);k<vpMath::rad(30);k+=vpMath::rad(10)){
80  for(double l=0.5;l<1.5;l+=0.1){
81  sum+=test(i,j,l,k);
82  }
83  }
84 
85  }
86  }
87 
88 
89  if(sum<0) return -1;
90  else return 0;
91 }
92 
93 int test(double x,double y,double z,double alpha){
94  //intial pose
95  vpHomogeneousMatrix cMo(x,y,z,-vpMath::rad(0),vpMath::rad(0),alpha);
96  //Desired pose
98 
99  //init and run the simulation
100  init(cMo,cdMo);
101  vpMatrix mat = execute();
102 
103  if(fabs(mat[0][0]-(-1)) > numeric_limits<double>::epsilon()*1e10) return -1;
104  if(fabs(mat[0][1]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
105  if(fabs(mat[0][2]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
106 
107  if(fabs(mat[1][0]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
108  if(fabs(mat[1][1]-(-1)) > numeric_limits<double>::epsilon()*1e10) return -1;
109  if(fabs(mat[1][2]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
110 
111  if(fabs(mat[2][0]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
112  if(fabs(mat[2][1]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
113  if(fabs(mat[2][2]-(-1)) > numeric_limits<double>::epsilon()*1e10) return -1;
114  if(fabs(mat[2][5]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
115 
116  if(fabs(mat[3][0]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
117  if(fabs(mat[3][1]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
118  if(fabs(mat[3][2]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
119  if(fabs(mat[3][5]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
120 
121  if(fabs(mat[4][0]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
122  if(fabs(mat[4][1]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
123  if(fabs(mat[4][2]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
124  if(fabs(mat[4][5]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
125 
126  if(fabs(mat[5][0]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
127  if(fabs(mat[5][1]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
128  if(fabs(mat[5][2]-(0)) > numeric_limits<double>::epsilon()*1e10) return -1;
129  if(fabs(mat[5][5]-(-1)) > numeric_limits<double>::epsilon()*1e10) return -1;
130 
131  return 0;
132 }
133 
134 
135 
136 
137 
138 void initScene(){
139  vector<vpPoint> src_pts;
140  vector<vpPoint> dst_pts;
141 
142  double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
143  double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
144  int nbpoints = 4;
145 
146  for (int i = 0 ; i < nbpoints ; i++){
147  vpPoint p;
148  p.setWorldCoordinates(x[i],y[i],0.0);
149  p.track(cMo) ;
150  src_pts.push_back(p);
151  }
152 
154  src.fromVector(src_pts);
155  for (int i = 0 ; i < nbpoints ; i++){
156  vpPoint p;
157  p.setWorldCoordinates(x[i],y[i],0.0);
158  p.track(cdMo) ;
159  dst_pts.push_back(p);
160  }
162  dst.fromVector(dst_pts);
163 
164 }
165 
166 
167 void init(vpHomogeneousMatrix& _cMo, vpHomogeneousMatrix& _cdMo)
168 {
169  cMo = _cMo;
170  cdMo = _cdMo;
171  interaction_type = vpServo::CURRENT;
172 
173 
174  initScene(); //initialize graphical scene (for interface)
175 }
176 
177 vpMatrix execute(){
178  vpServo task;
180  //A,B,C parameters of source and destination plane
181  double A; double B; double C;
182  double Ad; double Bd; double Cd;
183  //init main object: using moments up to order 6
184 
185  //Initializing values from regular plane (with ax+by+cz=d convention)
186  vpPlane pl;
187  pl.setABCD(0,0,1.0,0);
188  pl.changeFrame(cMo);
189  _planeToABC(pl,A,B,C);
190 
191  pl.setABCD(0,0,1.0,0);
192  pl.changeFrame(cdMo);
193  _planeToABC(pl,Ad,Bd,Cd);
194 
195  //extracting initial position (actually we only care about Zdst)
197  cdMo.extract(vec);
198 
200  //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
203  //same thing with common features
204  vpFeatureMomentCommon featureMoments(moments);
205  vpFeatureMomentCommon featureMomentsDes(momentsDes);
206 
207  moments.updateAll(src);
208  momentsDes.updateAll(dst);
209 
210  featureMoments.updateAll(A,B,C);
211  featureMomentsDes.updateAll(Ad,Bd,Cd);
212 
213  //setup the interaction type
214  task.setInteractionMatrixType(interaction_type) ;
216  task.addFeature(featureMoments.getFeatureGravityNormalized(),featureMomentsDes.getFeatureGravityNormalized());
217  task.addFeature(featureMoments.getFeatureAn(),featureMomentsDes.getFeatureAn());
218  //the moments are different in case of a symmetric object
219  task.addFeature(featureMoments.getFeatureCInvariant(),featureMomentsDes.getFeatureCInvariant(),(1 << 10) | (1 << 11));
220  task.addFeature(featureMoments.getFeatureAlpha(),featureMomentsDes.getFeatureAlpha());
221 
222  task.setLambda(0.4) ;
223 
224  task.computeControlLaw();
225  vpMatrix mat = task.computeInteractionMatrix();
226  task.kill();
227  return mat;
228 }
229 
230 
231 void _planeToABC(vpPlane& pl, double& A,double& B, double& C){
232 
233  A=-pl.getA()/pl.getD();
234  B=-pl.getB()/pl.getD();
235  C=-pl.getC()/pl.getD();
236 }
237 
Definition of the vpMatrix class.
Definition: vpMatrix.h:96
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
static double getAlpha(vpMomentObject &objec)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
Definition: vpServo.cpp:444
This class allows to access common vpFeatureMoments in a pre-filled database.
void setLambda(double _lambda)
set the gain lambda
Definition: vpServo.h:253
Class for generic objects.
void track(const vpHomogeneousMatrix &cMo)
void setType(vpObjectType type)
void setABCD(const double A, const double B, const double C, const double D)
Definition: vpPlane.h:99
Class that defines what is a point.
Definition: vpPoint.h:65
vpMatrix computeInteractionMatrix()
compute the interaction matrix related to the set of visual features
Definition: vpServo.cpp:590
void kill()
destruction (memory deallocation if required)
Definition: vpServo.cpp:177
vpColVector computeControlLaw()
compute the desired control law
Definition: vpServo.cpp:883
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:352
static std::vector< double > getMu3(vpMomentObject &object)
void extract(vpRotationMatrix &R) const
vpServoIteractionMatrixType
Definition: vpServo.h:169
void fromVector(std::vector< vpPoint > &points)
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
Definition: vpServo.cpp:509
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
Definition: vpMath.h:100
void updateAll(vpMomentObject &object)
This class initializes and allows access to commonly used moments.
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
double getB() const
Definition: vpPlane.h:114
double getA() const
Definition: vpPlane.h:112
double getC() const
Definition: vpPlane.h:116
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
Class required to compute the visual servoing control law descbribed in and .
Definition: vpServo.h:153
vpFeatureMomentAreaNormalized & getFeatureAn()
Class that consider the case of a translation vector.
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.
Definition: vpServo.cpp:214
double getD() const
Definition: vpPlane.h:118
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74