ViSP
servoMomentImage.cpp
1 /****************************************************************************
2  *
3  * $Id: servoMomentImage.cpp 4670 2014-02-17 08:59:05Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 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 an image as object
36  * container
37  *
38  * Authors:
39  * Filip Novotny
40  * Manikandan.B
41  *****************************************************************************/
42 
48 #define PRINT_CONDITION_NUMBER
49 
50 #include <visp/vpDebug.h>
51 #include <visp/vpConfig.h>
52 #include <iostream>
53 #include <visp/vpHomogeneousMatrix.h>
54 #include <visp/vpMomentObject.h>
55 #include <visp/vpMomentDatabase.h>
56 #include <visp/vpMomentCommon.h>
57 #include <visp/vpFeatureMomentCommon.h>
58 #include <visp/vpDisplayX.h>
59 #include <visp/vpDisplayGTK.h>
60 #include <visp/vpDisplayGDI.h>
61 #include <visp/vpDisplayOpenCV.h>
62 #include <visp/vpCameraParameters.h>
63 #include <visp/vpIoTools.h>
64 #include <visp/vpMath.h>
65 #include <visp/vpHomogeneousMatrix.h>
66 #include <visp/vpServo.h>
67 #include <visp/vpDebug.h>
68 #include <visp/vpFeatureBuilder.h>
69 #include <visp/vpFeaturePoint.h>
70 #include <visp/vpRobotCamera.h>
71 #include <visp/vpImageSimulator.h>
72 #include <visp/vpPlane.h>
73 #include <visp/vpPoseVector.h>
74 #include <visp/vpPlot.h>
75 
76 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
77 // Robot simulator used in this example is not available
78 int main()
79 {
80  std::cout << "Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
81  std::cout << "You should install pthread third-party library." << std::endl;
82 }
83 // No display available
84 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
85 int main()
86 {
87  std::cout << "Can't run this example since no display capability is available." << std::endl;
88  std::cout << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
89 }
90 #else
91 
92 //setup robot parameters
93 void paramRobot();
94 
95 //update moment objects and interface
96 void refreshScene(vpMomentObject &obj);
97 //initialize scene in the interface
98 void initScene();
99 //initialize the moment features
100 void initFeatures();
101 
102 void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
103 void execute(unsigned int nbIter); //launch the simulation
104 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
105 double error();
106 void planeToABC(vpPlane& pl, double& A,double& B, double& C);
107 void paramRobot();
108 
109 void init_visp_plot(vpPlot& );
110 
111 int main()
112 {
113  try {
114  //intial pose
115  vpHomogeneousMatrix cMo(-0.1,-0.1,1.5,-vpMath::rad(20),-vpMath::rad(20),-vpMath::rad(30));
116  //Desired pose
118 
119  //init the simulation
120  init(cMo,cdMo);
121 
122  execute(1500);
123  return 0;
124  }
125  catch(vpException e) {
126  std::cout << "Catch an exception: " << e << std::endl;
127  return 1;
128  }
129 }
130 
131 
132 //init the right display
133 #if defined VISP_HAVE_X11
134 vpDisplayX displayInt;
135 #elif defined VISP_HAVE_OPENCV
136 vpDisplayOpenCV displayInt;
137 #elif defined VISP_HAVE_GDI
138 vpDisplayGDI displayInt;
139 #elif defined VISP_HAVE_D3D9
140 vpDisplayD3D displayInt;
141 #elif defined VISP_HAVE_GTK
142 vpDisplayGTK displayInt;
143 #endif
144 
145 //start and destination positioning matrices
148 
149 vpRobotCamera robot;//robot used in this simulation
150 vpImage<vpRGBa> Iint(480,640, 0);//internal image used for interface display
151 vpServo task; //servoing task
152 vpCameraParameters cam;//robot camera parameters
153 double _error; //current error
154 vpImageSimulator imsim;//image simulator used to simulate the perspective-projection camera
155 
156 //several images used in the simulation
157 vpImage<unsigned char> cur_img(480,640, 0);
158 vpImage<unsigned char> src_img(480,640, 0);
159 vpImage<unsigned char> dst_img(480,640, 0);
160 vpImage<vpRGBa> start_img(480,640, 0);
161 vpServo::vpServoIteractionMatrixType interaction_type; //current or desired
162 //source and destination objects for moment manipulation
163 vpMomentObject src(6);
164 vpMomentObject dst(6);
165 
166 //moment sets and their corresponding features
167 vpMomentCommon *moments;
168 vpMomentCommon *momentsDes;
169 vpFeatureMomentCommon *featureMoments;
170 vpFeatureMomentCommon *featureMomentsDes;
171 
172 using namespace std;
173 
174 
175 void initScene(){
176  vpColVector X[4];
177  for (int i = 0; i < 4; i++) X[i].resize(3);
178  X[0][0] = -0.2;
179  X[0][1] = -0.1;
180  X[0][2] = 0;
181 
182  X[1][0] = 0.2;
183  X[1][1] = -0.1;
184  X[1][2] = 0;
185 
186  X[2][0] = 0.2;
187  X[2][1] = 0.1;
188  X[2][2] = 0;
189 
190  X[3][0] = -0.2;
191  X[3][1] = 0.1;
192  X[3][2] = 0;
193  //init source and destination images
194  vpImage<unsigned char> tmp_img(480,640,255);
195  vpImage<vpRGBa> tmp_start_img(480,640,vpRGBa(255,0,0));
196 
197  vpImageSimulator imsim_start;
199  imsim_start.init(tmp_start_img, X);
200  imsim_start.setCameraPosition(cdMo);
201  imsim_start.getImage(start_img,cam);
202 
204  imsim.init(tmp_img, X);
205 
206  imsim.setCameraPosition(cMo);
207  imsim.getImage(src_img,cam);
208 
210  src.fromImage(src_img,128,cam);
211 
212 
214  imsim.setCameraPosition(cdMo);
215  imsim.getImage(dst_img,cam);
216  dst.fromImage(dst_img,128,cam);
217 
218 }
219 
220 void refreshScene(vpMomentObject &obj){
221  cur_img = 0;
222  imsim.setCameraPosition(cMo);
223  imsim.getImage(cur_img,cam);
224  obj.fromImage(cur_img,128,cam);
225 }
226 
227 void init(vpHomogeneousMatrix& _cMo, vpHomogeneousMatrix& _cdMo)
228 {
229  cMo = _cMo; //init source matrix
230  cdMo = _cdMo; //init destination matrix
231 
232  interaction_type = vpServo::CURRENT;//use interaction matrix for current position
233 
234  displayInt.init(Iint,700,0, "Visual servoing with moments") ;
235 
236  paramRobot(); //set up robot parameters
237 
239  initScene(); //initialize graphical scene (for interface)
240  initFeatures();//initialize moment features
241 }
242 
243 void initFeatures(){
244  //A,B,C parameters of source and destination plane
245  double A; double B; double C;
246  double Ad; double Bd; double Cd;
247  //init main object: using moments up to order 5
248 
249  //Initializing values from regular plane (with ax+by+cz=d convention)
250  vpPlane pl;
251  pl.setABCD(0,0,1.0,0);
252  pl.changeFrame(cMo);
253  planeToABC(pl,A,B,C);
254 
255  pl.setABCD(0,0,1.0,0);
256  pl.changeFrame(cdMo);
257  planeToABC(pl,Ad,Bd,Cd);
258 
259  //extracting initial position (actually we only care about Zdst)
261  cdMo.extract(vec);
262 
264  //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
267  //same thing with common features
268  featureMoments = new vpFeatureMomentCommon(*moments);
269  featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
270 
271  moments->updateAll(src);
272  momentsDes->updateAll(dst);
273 
274  featureMoments->updateAll(A,B,C);
275  featureMomentsDes->updateAll(Ad,Bd,Cd);
276 
277  //setup the interaction type
278  task.setInteractionMatrixType(interaction_type) ;
280  task.addFeature(featureMoments->getFeatureGravityNormalized(),featureMomentsDes->getFeatureGravityNormalized());
281  task.addFeature(featureMoments->getFeatureAn(),featureMomentsDes->getFeatureAn());
282  //the moments are different in case of a symmetric object
283  task.addFeature(featureMoments->getFeatureCInvariant(),featureMomentsDes->getFeatureCInvariant(),(1 << 10) | (1 << 11));
284  task.addFeature(featureMoments->getFeatureAlpha(),featureMomentsDes->getFeatureAlpha());
285 
286  task.setLambda(1.);
287 }
288 
289 void execute(unsigned int nbIter){
290 
291  vpPlot ViSP_plot;
292  init_visp_plot(ViSP_plot); // Initialize plot object
293 
294  //init main object: using moments up to order 6
295  vpMomentObject obj(6);
296  //setting object type (disrete, continuous[form polygon])
298 
299  vpTRACE("Display task information " ) ;
300  task.print() ;
301 
302  vpDisplay::display(Iint);
303  vpDisplay::flush(Iint);
304  unsigned int iter=0;
305  double t=0;
306  robot.setPosition(cMo);
307  float sampling_time = 0.010f; // Sampling period in seconds
308  robot.setSamplingTime(sampling_time);
309 
310  // For plotting
311  vpPoseVector currentpose;
312  vpColVector err_features;
313 
315  while(iter++<nbIter ){
316 
317  vpColVector v ;
318  t = vpTime::measureTimeMs();
319  //get the cMo
320  robot.getPosition(cMo);
321  currentpose.buildFrom(cMo); // For plot
322  //setup the plane in A,B,C style
323  vpPlane pl;
324  double A,B,C;
325  pl.setABCD(0,0,1.0,0);
326  pl.changeFrame(cMo);
327  planeToABC(pl,A,B,C);
328 
329  //track points, draw points and add refresh our object
330  refreshScene(obj);
331  //this is the most important thing to do: update our moments
332  moments->updateAll(obj);
333  //and update our features. Do it in that order. Features need to use the information computed by moments
334  featureMoments->updateAll(A,B,C);
335  //some graphics again
336  imsim.setCameraPosition(cMo);
337 
338  Iint = start_img;
339 
340  imsim.getImage(Iint,cam);
341  vpDisplay::display(Iint) ;
342 
343  vpDisplay::flush(Iint);
344 
345  if (iter == 1)
346  vpDisplay::getClick(Iint) ;
347  v = task.computeControlLaw() ;
348  //pilot robot using position control. The displacement is t*v with t=10ms step
349  //robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
350 
351  err_features = task.error;
352  std::cout<<" || s - s* || = "<<task.error.sumSquare()<<std::endl;
353 
355  vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
356 
357  ViSP_plot.plot(0,iter, v);
358  ViSP_plot.plot(1,iter,currentpose); // Plot the velocities
359  ViSP_plot.plot(2, iter,err_features); //cMo as translations and theta_u
360 
361  _error = ( task.getError() ).sumSquare();
362 
363  #if defined(PRINT_CONDITION_NUMBER)
364  /*
365  * Condition number of interaction matrix
366  */
367  vpMatrix Linteraction = task.L;
368  vpMatrix tmpry,U;
369  vpColVector singularvals;
370  Linteraction.svd(singularvals, tmpry);
371  double condno = static_cast<double>(singularvals.getMaxValue()/singularvals.getMinValue());
372  std::cout<<"Condition Number: "<<condno<<std::endl;
373  #endif
374 
375  }
376 
377  task.kill();
378 
379  vpTRACE("\n\nClick in the internal view window to end...");
380  vpDisplay::getClick(Iint) ;
381 
382  delete moments;
383  delete momentsDes;
384  delete featureMoments;
385  delete featureMomentsDes;
386 }
387 
388 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type){interaction_type=type;}
389 double error(){return _error;}
390 
391 
392 
393 void planeToABC(vpPlane& pl, double& A,double& B, double& C){
394  if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
395  std::cout << "Invalid position:" << std::endl;
396  std::cout << cMo << std::endl;
397  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
398  throw vpException(vpException::divideByZeroError,"invalid position!");
399  }
400  A=-pl.getA()/pl.getD();
401  B=-pl.getB()/pl.getD();
402  C=-pl.getC()/pl.getD();
403 }
404 
405 void paramRobot(){
406  cam = vpCameraParameters(640,480,320,240);
407 }
408 
409 void
410 init_visp_plot(vpPlot& ViSP_plot) {
411  /* -------------------------------------
412  * Initialize ViSP Plotting
413  * -------------------------------------
414  */
415  const unsigned int NbGraphs = 3; // No. of graphs
416  const unsigned int NbCurves_in_graph[NbGraphs] = {6,6,6}; // Curves in each graph
417 
418  ViSP_plot.init(NbGraphs , 800, 800, 10, 10, "Visual Servoing results...");
419 
420  vpColor Colors[6] = {\
421  // Colour for s1, s2, s3, in 1st plot
424  };
425 
426  for (unsigned int p = 0; p<NbGraphs; p++) {
427  ViSP_plot.initGraph(p,NbCurves_in_graph[p]);
428  for (unsigned int c = 0; c<NbCurves_in_graph[p]; c++)
429  ViSP_plot.setColor(p,c,Colors[c]);
430  }
431 
432  ViSP_plot.setTitle(0,"Robot velocities");
433  ViSP_plot.setLegend(0, 0, "v_x");
434  ViSP_plot.setLegend(0, 1, "v_y");
435  ViSP_plot.setLegend(0, 2, "v_z");
436  ViSP_plot.setLegend(0, 3, "w_x");
437  ViSP_plot.setLegend(0, 4, "w_y");
438  ViSP_plot.setLegend(0, 5, "w_z");
439 
440  ViSP_plot.setTitle(1,"Camera pose cMo");
441  ViSP_plot.setLegend(1, 0, "tx");
442  ViSP_plot.setLegend(1, 1, "ty");
443  ViSP_plot.setLegend(1, 2, "tz");
444  ViSP_plot.setLegend(1, 3, "tu_x");
445  ViSP_plot.setLegend(1, 4, "tu_y");
446  ViSP_plot.setLegend(1, 5, "tu_z");
447 
448  ViSP_plot.setTitle(2,"Error in visual features: ");
449  ViSP_plot.setLegend(2, 0, "x_n");
450  ViSP_plot.setLegend(2, 1, "y_n");
451  ViSP_plot.setLegend(2, 2, "a_n");
452  ViSP_plot.setLegend(2, 3, "sx");
453  ViSP_plot.setLegend(2, 4, "sy");
454  ViSP_plot.setLegend(2, 5, "alpha");
455 }
456 #endif
Definition of the vpMatrix class.
Definition: vpMatrix.h:98
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setColor(const unsigned int graphNum, const unsigned int curveNum, vpColor color)
Definition: vpPlot.cpp:267
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
#define vpTRACE
Definition: vpDebug.h:418
void setPosition(const vpHomogeneousMatrix &cMw)
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
Class to define colors available for display functionnalities.
Definition: vpColor.h:125
double getMinValue() const
Definition: vpMatrix.cpp:4224
Define the X11 console to display images.
Definition: vpDisplayX.h:152
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:449
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
Definition: vpException.h:76
void setABCD(const double a, const double b, const double c, const double d)
Definition: vpPlane.h:102
Class for generic objects.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void setLegend(const unsigned int graphNum, const unsigned int curveNum, const char *legend)
Definition: vpPlot.cpp:477
void plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
Definition: vpPlot.cpp:291
static double measureTimeMs()
Definition: vpTime.cpp:86
void setTitle(const unsigned int graphNum, const char *title)
Definition: vpPlot.cpp:428
static int wait(double t0, double t)
Definition: vpTime.cpp:149
double sumSquare() const
Definition: vpMatrix.cpp:868
static const vpColor green
Definition: vpColor.h:170
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
Class that defines a RGB 32 bits structure.
Definition: vpRGBa.h:68
static const vpColor red
Definition: vpColor.h:167
vpPoseVector buildFrom(const vpHomogeneousMatrix &M)
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
static const vpColor orange
Definition: vpColor.h:177
virtual void setSamplingTime(const double &delta_t)
Display for windows using Direct3D.
Definition: vpDisplayD3D.h:109
void kill()
Definition: vpServo.cpp:189
void setCameraPosition(const vpHomogeneousMatrix &cMt)
vpColVector getError() const
Definition: vpServo.h:257
static const vpColor cyan
Definition: vpColor.h:176
void svd(vpColVector &w, vpMatrix &v)
Definition: vpMatrix.cpp:1822
vpColVector computeControlLaw()
Definition: vpServo.cpp:902
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void setInterpolationType(const vpInterpolationType interplt)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:376
Class that defines the simplest robot: a free flying camera.
static std::vector< double > getMu3(vpMomentObject &object)
vpMatrix L
Interaction matrix.
Definition: vpServo.h:460
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
The vpDisplayOpenCV allows to display image using the opencv library.
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:370
Class which enables to project an image in the 3D space and get the view of a virtual camera...
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
void extract(vpRotationMatrix &R) const
void init(const unsigned int nbGraph, const unsigned int height=700, const unsigned int width=700, const int x=-1, const int y=-1, const char *title=NULL)
Definition: vpPlot.cpp:108
vpServoIteractionMatrixType
Definition: vpServo.h:187
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:522
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
Definition: vpMath.h:100
void updateAll(vpMomentObject &object)
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:208
void getPosition(vpHomogeneousMatrix &cMw) const
This class initializes and allows access to commonly used moments.
static double getAlpha(vpMomentObject &object)
double getMaxValue() const
Definition: vpMatrix.cpp:4238
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
double getB() const
Definition: vpPlane.h:117
void setType(vpObjectType input_type)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:251
double getA() const
Definition: vpPlane.h:115
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:117
double getC() const
Definition: vpPlane.h:119
virtual bool getClick(bool blocking=true)=0
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:67
static const vpColor purple
Definition: vpColor.h:178
vpFeatureMomentAreaNormalized & getFeatureAn()
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:220
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
vpColVector error
Definition: vpServo.h:464
Class that consider the case of a translation vector.
double getD() const
Definition: vpPlane.h:121
static const vpColor blue
Definition: vpColor.h:173