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