ViSP
 All Classes Functions Variables Enumerations Enumerator Friends Groups Pages
servoMomentImage.cpp
1 /****************************************************************************
2  *
3  * $Id: servoMomentImage.cpp 4056 2013-01-05 13:04:42Z fspindle $
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 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 //setup robot parameters
77 void paramRobot();
78 
79 //update moment objects and interface
80 void refreshScene(vpMomentObject &obj);
81 //initialize scene in the interface
82 void initScene();
83 //initialize the moment features
84 void initFeatures();
85 
86 void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
87 void execute(unsigned int nbIter); //launch the simulation
88 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
89 double error();
90 void _planeToABC(vpPlane& pl, double& A,double& B, double& C);
91 void paramRobot();
92 
93 #if !defined(WIN32) && !defined(VISP_HAVE_PTHREAD)
94 // Robot simulator used in this example is not available
95 int main()
96 {
97  std::cout << "Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
98  std::cout << "You should install pthread third-party library." << std::endl;
99 }
100 // No display available
101 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && !defined(VISP_HAVE_GTK)
102 int main()
103 {
104  std::cout << "Can't run this example since no display capability is available." << std::endl;
105  std::cout << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
106 }
107 #else
108 void init_visp_plot(vpPlot& );
109 
110 int main()
111 {
112  //intial pose
113  vpHomogeneousMatrix cMo(-0.1,-0.1,1.5,-vpMath::rad(20),-vpMath::rad(20),-vpMath::rad(30));
114  //Desired pose
116 
117  //init the simulation
118  init(cMo,cdMo);
119 
120  execute(1500);
121  return 0;
122 }
123 
124 
125 //init the right display
126 #if defined VISP_HAVE_X11
127 vpDisplayX displayInt;
128 #elif defined VISP_HAVE_OPENCV
129 vpDisplayOpenCV displayInt;
130 #elif defined VISP_HAVE_GDI
131 vpDisplayGDI displayInt;
132 #elif defined VISP_HAVE_D3D9
133 vpDisplayD3D displayInt;
134 #elif defined VISP_HAVE_GTK
135 vpDisplayGTK displayInt;
136 #endif
137 
138 //start and destination positioning matrices
141 
142 vpRobotCamera robot;//robot used in this simulation
143 vpImage<vpRGBa> Iint(480,640, 0);//internal image used for interface display
144 vpServo task; //servoing task
145 vpCameraParameters cam;//robot camera parameters
146 double _error; //current error
147 vpImageSimulator imsim;//image simulator used to simulate the perspective-projection camera
148 
149 //several images used in the simulation
150 vpImage<unsigned char> cur_img(480,640, 0);
151 vpImage<unsigned char> src_img(480,640, 0);
152 vpImage<unsigned char> dst_img(480,640, 0);
153 vpImage<vpRGBa> start_img(480,640, 0);
154 vpServo::vpServoIteractionMatrixType interaction_type; //current or desired
155 //source and destination objects for moment manipulation
156 vpMomentObject src(6);
157 vpMomentObject dst(6);
158 
159 //moment sets and their corresponding features
160 vpMomentCommon *moments;
161 vpMomentCommon *momentsDes;
162 vpFeatureMomentCommon *featureMoments;
163 vpFeatureMomentCommon *featureMomentsDes;
164 
165 using namespace std;
166 
167 
168 void initScene(){
169  vpColVector X[4];
170  for (int i = 0; i < 4; i++) X[i].resize(3);
171  X[0][0] = -0.2;
172  X[0][1] = -0.1;
173  X[0][2] = 0;
174 
175  X[1][0] = 0.2;
176  X[1][1] = -0.1;
177  X[1][2] = 0;
178 
179  X[2][0] = 0.2;
180  X[2][1] = 0.1;
181  X[2][2] = 0;
182 
183  X[3][0] = -0.2;
184  X[3][1] = 0.1;
185  X[3][2] = 0;
186  //init source and destination images
187  vpImage<unsigned char> tmp_img(480,640,255);
188  vpImage<vpRGBa> tmp_start_img(480,640,vpRGBa(255,0,0));
189 
190  vpImageSimulator imsim_start;
192  imsim_start.init(tmp_start_img, X);
193  imsim_start.setCameraPosition(cdMo);
194  imsim_start.getImage(start_img,cam);
195 
197  imsim.init(tmp_img, X);
198 
199  imsim.setCameraPosition(cMo);
200  imsim.getImage(src_img,cam);
201 
203  src.fromImage(src_img,128,cam);
204 
205 
207  imsim.setCameraPosition(cdMo);
208  imsim.getImage(dst_img,cam);
209  dst.fromImage(dst_img,128,cam);
210 
211 }
212 
213 void refreshScene(vpMomentObject &obj){
214  cur_img = 0;
215  imsim.setCameraPosition(cMo);
216  imsim.getImage(cur_img,cam);
217  obj.fromImage(cur_img,128,cam);
218 }
219 
220 void init(vpHomogeneousMatrix& _cMo, vpHomogeneousMatrix& _cdMo)
221 {
222  cMo = _cMo; //init source matrix
223  cdMo = _cdMo; //init destination matrix
224 
225  interaction_type = vpServo::CURRENT;//use interaction matrix for current position
226 
227  displayInt.init(Iint,700,0, "Visual servoing with moments") ;
228 
229  paramRobot(); //set up robot parameters
230 
232  initScene(); //initialize graphical scene (for interface)
233  initFeatures();//initialize moment features
234 }
235 
236 void initFeatures(){
237  //A,B,C parameters of source and destination plane
238  double A; double B; double C;
239  double Ad; double Bd; double Cd;
240  //init main object: using moments up to order 5
241 
242  //Initializing values from regular plane (with ax+by+cz=d convention)
243  vpPlane pl;
244  pl.setABCD(0,0,1.0,0);
245  pl.changeFrame(cMo);
246  _planeToABC(pl,A,B,C);
247 
248  pl.setABCD(0,0,1.0,0);
249  pl.changeFrame(cdMo);
250  _planeToABC(pl,Ad,Bd,Cd);
251 
252  //extracting initial position (actually we only care about Zdst)
254  cdMo.extract(vec);
255 
257  //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
260  //same thing with common features
261  featureMoments = new vpFeatureMomentCommon(*moments);
262  featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
263 
264  moments->updateAll(src);
265  momentsDes->updateAll(dst);
266 
267  featureMoments->updateAll(A,B,C);
268  featureMomentsDes->updateAll(Ad,Bd,Cd);
269 
270  //setup the interaction type
271  task.setInteractionMatrixType(interaction_type) ;
273  task.addFeature(featureMoments->getFeatureGravityNormalized(),featureMomentsDes->getFeatureGravityNormalized());
274  task.addFeature(featureMoments->getFeatureAn(),featureMomentsDes->getFeatureAn());
275  //the moments are different in case of a symmetric object
276  task.addFeature(featureMoments->getFeatureCInvariant(),featureMomentsDes->getFeatureCInvariant(),(1 << 10) | (1 << 11));
277  task.addFeature(featureMoments->getFeatureAlpha(),featureMomentsDes->getFeatureAlpha());
278 
279  task.setLambda(1.);
280 }
281 
282 void execute(unsigned int nbIter){
283 
284  vpPlot ViSP_plot;
285  init_visp_plot(ViSP_plot); // Initialize plot object
286 
287  //init main object: using moments up to order 6
288  vpMomentObject obj(6);
289  //setting object type (disrete, continuous[form polygon])
291 
292  vpTRACE("Display task information " ) ;
293  task.print() ;
294 
295  vpDisplay::display(Iint);
296  vpDisplay::flush(Iint);
297  unsigned int iter=0;
298  double t=0;
299  robot.setPosition(cMo);
300  float sampling_time = 0.010f; // Sampling period in seconds
301  robot.setSamplingTime(sampling_time);
302 
303  // For plotting
304  vpPoseVector currentpose;
305  vpColVector err_features;
306 
308  while(iter++<nbIter ){
309 
310  vpColVector v ;
311  t = vpTime::measureTimeMs();
312  //get the cMo
313  robot.getPosition(cMo);
314  currentpose.buildFrom(cMo); // For plot
315  //setup the plane in A,B,C style
316  vpPlane pl;
317  double A,B,C;
318  pl.setABCD(0,0,1.0,0);
319  pl.changeFrame(cMo);
320  _planeToABC(pl,A,B,C);
321 
322  //track points, draw points and add refresh our object
323  refreshScene(obj);
324  //this is the most important thing to do: update our moments
325  moments->updateAll(obj);
326  //and update our features. Do it in that order. Features need to use the information computed by moments
327  featureMoments->updateAll(A,B,C);
328  //some graphics again
329  imsim.setCameraPosition(cMo);
330 
331  Iint = start_img;
332 
333  imsim.getImage(Iint,cam);
334  vpDisplay::display(Iint) ;
335 
336  vpDisplay::flush(Iint);
337 
338  if (iter == 1)
339  vpDisplay::getClick(Iint) ;
340  v = task.computeControlLaw() ;
341  //pilot robot using position control. The displacement is t*v with t=10ms step
342  //robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
343 
344  err_features = task.error;
345  std::cout<<" || s - s* || = "<<task.error.sumSquare()<<std::endl;
346 
348  vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
349 
350  ViSP_plot.plot(0,iter, v);
351  ViSP_plot.plot(1,iter,currentpose); // Plot the velocities
352  ViSP_plot.plot(2, iter,err_features); //cMo as translations and theta_u
353 
354  _error = ( task.getError() ).sumSquare();
355 
356  #if defined(PRINT_CONDITION_NUMBER)
357  /*
358  * Condition number of interaction matrix
359  */
360  vpMatrix Linteraction = task.L;
361  vpMatrix tmpry,U;
362  vpColVector singularvals;
363  Linteraction.svd(singularvals, tmpry);
364  double condno = static_cast<double>(singularvals.getMaxValue()/singularvals.getMinValue());
365  std::cout<<"Condition Number: "<<condno<<std::endl;
366  #endif
367 
368  }
369 
370  task.kill();
371 
372  vpTRACE("\n\nClick in the internal view window to end...");
373  vpDisplay::getClick(Iint) ;
374 
375  delete moments;
376  delete momentsDes;
377  delete featureMoments;
378  delete featureMomentsDes;
379 }
380 
381 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type){interaction_type=type;}
382 double error(){return _error;}
383 
384 
385 
386 void _planeToABC(vpPlane& pl, double& A,double& B, double& C){
387  if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
388  std::cout << "Invalid position:" << std::endl;
389  std::cout << cMo << std::endl;
390  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
391  throw vpException(vpException::divideByZeroError,"invalid position!");
392  }
393  A=-pl.getA()/pl.getD();
394  B=-pl.getB()/pl.getD();
395  C=-pl.getC()/pl.getD();
396 }
397 
398 void paramRobot(){
399  cam = vpCameraParameters(640,480,320,240);
400 }
401 
402 void
403 init_visp_plot(vpPlot& ViSP_plot) {
404  /* -------------------------------------
405  * Initialize ViSP Plotting
406  * -------------------------------------
407  */
408  const unsigned int NbGraphs = 3; // No. of graphs
409  const unsigned int NbCurves_in_graph[NbGraphs] = {6,6,6}; // Curves in each graph
410 
411  ViSP_plot.init(NbGraphs , 800, 800, 10, 10, "Visual Servoing results...");
412 
413  vpColor Colors[6] = {\
414  // Colour for s1, s2, s3, in 1st plot
416  vpColor::orange, vpColor::cyan,vpColor::purple
417  };
418 
419  for (unsigned int p = 0; p<NbGraphs; p++) {
420  ViSP_plot.initGraph(p,NbCurves_in_graph[p]);
421  for (unsigned int c = 0; c<NbCurves_in_graph[p]; c++)
422  ViSP_plot.setColor(p,c,Colors[c]);
423  }
424 
425  ViSP_plot.setTitle(0,"Robot velocities");
426  ViSP_plot.setLegend(0, 0, "v_x");
427  ViSP_plot.setLegend(0, 1, "v_y");
428  ViSP_plot.setLegend(0, 2, "v_z");
429  ViSP_plot.setLegend(0, 3, "w_x");
430  ViSP_plot.setLegend(0, 4, "w_y");
431  ViSP_plot.setLegend(0, 5, "w_z");
432 
433  ViSP_plot.setTitle(1,"Camera pose cMo");
434  ViSP_plot.setLegend(1, 0, "tx");
435  ViSP_plot.setLegend(1, 1, "ty");
436  ViSP_plot.setLegend(1, 2, "tz");
437  ViSP_plot.setLegend(1, 3, "tu_x");
438  ViSP_plot.setLegend(1, 4, "tu_y");
439  ViSP_plot.setLegend(1, 5, "tu_z");
440 
441  ViSP_plot.setTitle(2,"Error in visual features: ");
442  ViSP_plot.setLegend(2, 0, "x_n");
443  ViSP_plot.setLegend(2, 1, "y_n");
444  ViSP_plot.setLegend(2, 2, "a_n");
445  ViSP_plot.setLegend(2, 3, "sx");
446  ViSP_plot.setLegend(2, 4, "sy");
447  ViSP_plot.setLegend(2, 5, "alpha");
448 }
449 #endif