Visual Servoing Platform  version 3.4.0
servoMomentImage.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 an image as object
33  * container
34  *
35  * Authors:
36  * Filip Novotny
37  * Manikandan.B
38  *****************************************************************************/
39 
45 #define PRINT_CONDITION_NUMBER
46 
47 #include <iostream>
48 #include <visp3/core/vpCameraParameters.h>
49 #include <visp3/core/vpHomogeneousMatrix.h>
50 #include <visp3/core/vpIoTools.h>
51 #include <visp3/core/vpMath.h>
52 #include <visp3/core/vpMomentCommon.h>
53 #include <visp3/core/vpMomentDatabase.h>
54 #include <visp3/core/vpMomentObject.h>
55 #include <visp3/core/vpPlane.h>
56 #include <visp3/core/vpPoseVector.h>
57 #include <visp3/gui/vpDisplayGDI.h>
58 #include <visp3/gui/vpDisplayGTK.h>
59 #include <visp3/gui/vpDisplayOpenCV.h>
60 #include <visp3/gui/vpDisplayX.h>
61 #include <visp3/gui/vpPlot.h>
62 #include <visp3/robot/vpImageSimulator.h>
63 #include <visp3/robot/vpSimulatorCamera.h>
64 #include <visp3/visual_features/vpFeatureBuilder.h>
65 #include <visp3/visual_features/vpFeatureMomentCommon.h>
66 #include <visp3/visual_features/vpFeaturePoint.h>
67 #include <visp3/vs/vpServo.h>
68 
69 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
70 // Robot simulator used in this example is not available
71 int main()
72 {
73  std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
74  "not available."
75  << std::endl;
76  std::cout << "You should install pthread third-party library." << std::endl;
77  return EXIT_SUCCESS;
78 }
79 // No display available
80 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
81  !defined(VISP_HAVE_GTK)
82 int main()
83 {
84  std::cout << "Can't run this example since no display capability is available." << std::endl;
85  std::cout << "You should install one of the following third-party library: "
86  "X11, OpenCV, GDI, GTK."
87  << std::endl;
88  return EXIT_SUCCESS;
89 }
90 #else
91 
92 #ifndef DOXYGEN_SHOULD_SKIP_THIS
93 class servoMoment
94 {
95 public:
96  servoMoment()
97  : m_width(640), m_height(480), m_cMo(), m_cdMo(), m_robot(), m_Iint(m_height, m_width, 0), m_task(), m_cam(),
98  m_error(0), m_imsim(), m_cur_img(m_height, m_width, 0), m_src_img(m_height, m_width, 0),
99  m_dst_img(m_height, m_width, 0), m_start_img(m_height, m_width, 0), m_interaction_type(), m_src(6), m_dst(6),
100  m_moments(NULL), m_momentsDes(NULL), m_featureMoments(NULL), m_featureMomentsDes(NULL), m_displayInt(NULL)
101  {
102  }
103  ~servoMoment()
104  {
105 #ifdef VISP_HAVE_DISPLAY
106  if (m_displayInt) {
107  delete m_displayInt;
108  }
109 #endif
110  delete m_moments;
111  delete m_momentsDes;
112  delete m_featureMoments;
113  delete m_featureMomentsDes;
114  }
115 
116  // setup robot parameters
117  void paramRobot() { m_cam = vpCameraParameters(600, 600, m_width / 2., m_height / 2.); }
118 
119  // update moment objects and interface
120  void refreshScene(vpMomentObject &obj)
121  {
122  m_cur_img = 0;
123  m_imsim.setCameraPosition(m_cMo);
124  m_imsim.getImage(m_cur_img, m_cam);
125  obj.fromImage(m_cur_img, 128, m_cam);
126  }
127 
128  // initialize scene in the interface
129  void initScene()
130  {
131  vpColVector X[4];
132  for (int i = 0; i < 4; i++)
133  X[i].resize(3);
134  X[0][0] = -0.2;
135  X[0][1] = -0.1;
136  X[0][2] = 0;
137 
138  X[1][0] = 0.2;
139  X[1][1] = -0.1;
140  X[1][2] = 0;
141 
142  X[2][0] = 0.2;
143  X[2][1] = 0.1;
144  X[2][2] = 0;
145 
146  X[3][0] = -0.2;
147  X[3][1] = 0.1;
148  X[3][2] = 0;
149  // init source and destination images
150  vpImage<unsigned char> tmp_img(m_height, m_width, 255);
151  vpImage<vpRGBa> tmp_start_img(m_height, m_width, vpRGBa(255, 0, 0));
152 
153  vpImageSimulator imsim_start;
155  imsim_start.init(tmp_start_img, X);
156  imsim_start.setCameraPosition(m_cdMo);
157  imsim_start.getImage(m_start_img, m_cam);
158 
159  m_imsim.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION);
160  m_imsim.init(tmp_img, X);
161 
162  m_imsim.setCameraPosition(m_cMo);
163  m_imsim.getImage(m_src_img, m_cam);
164 
165  m_src.setType(vpMomentObject::DENSE_FULL_OBJECT);
166  m_src.fromImage(m_src_img, 128, m_cam);
167 
168  m_dst.setType(vpMomentObject::DENSE_FULL_OBJECT);
169  m_imsim.setCameraPosition(m_cdMo);
170  m_imsim.getImage(m_dst_img, m_cam);
171  m_dst.fromImage(m_dst_img, 128, m_cam);
172  }
173 
174  // initialize the moment features
175  void initFeatures()
176  {
177  // A,B,C parameters of source and destination plane
178  double A;
179  double B;
180  double C;
181  double Ad;
182  double Bd;
183  double Cd;
184  // init main object: using moments up to order 5
185 
186  // Initializing values from regular plane (with ax+by+cz=d convention)
187  vpPlane pl;
188  pl.setABCD(0, 0, 1.0, 0);
189  pl.changeFrame(m_cMo);
190  planeToABC(pl, A, B, C);
191 
192  pl.setABCD(0, 0, 1.0, 0);
193  pl.changeFrame(m_cdMo);
194  planeToABC(pl, Ad, Bd, Cd);
195 
196  // extracting initial position (actually we only care about Zdst)
198  m_cdMo.extract(vec);
199 
202  // don't need to be specific, vpMomentCommon automatically loads
203  // Xg,Yg,An,Ci,Cj,Alpha moments
205  vpMomentCommon::getAlpha(m_dst), vec[2], true);
206  m_momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(m_dst), vpMomentCommon::getMu3(m_dst),
207  vpMomentCommon::getAlpha(m_dst), vec[2], true);
208  // same thing with common features
209  m_featureMoments = new vpFeatureMomentCommon(*m_moments);
210  m_featureMomentsDes = new vpFeatureMomentCommon(*m_momentsDes);
211 
212  m_moments->updateAll(m_src);
213  m_momentsDes->updateAll(m_dst);
214 
215  m_featureMoments->updateAll(A, B, C);
216  m_featureMomentsDes->updateAll(Ad, Bd, Cd);
217 
218  // setup the interaction type
219  m_task.setInteractionMatrixType(m_interaction_type);
222  m_task.addFeature(m_featureMoments->getFeatureGravityNormalized(),
223  m_featureMomentsDes->getFeatureGravityNormalized());
224  m_task.addFeature(m_featureMoments->getFeatureAn(), m_featureMomentsDes->getFeatureAn());
225  // the moments are different in case of a symmetric object
226  m_task.addFeature(m_featureMoments->getFeatureCInvariant(), m_featureMomentsDes->getFeatureCInvariant(),
227  (1 << 10) | (1 << 11));
228  m_task.addFeature(m_featureMoments->getFeatureAlpha(), m_featureMomentsDes->getFeatureAlpha());
229 
230  m_task.setLambda(1.);
231  }
232 
233  void init(vpHomogeneousMatrix &cMo, vpHomogeneousMatrix &cdMo)
234  {
235  m_cMo = cMo; // init source matrix
236  m_cdMo = cdMo; // init destination matrix
237 
238  m_interaction_type = vpServo::CURRENT; // use interaction matrix for current position
239 
240 #ifdef VISP_HAVE_DISPLAY
241  // init the right display
242 #if defined VISP_HAVE_X11
243  m_displayInt = new vpDisplayX;
244 #elif defined VISP_HAVE_OPENCV
245  m_displayInt = new vpDisplayOpenCV;
246 #elif defined VISP_HAVE_GDI
247  m_displayInt = new vpDisplayGDI;
248 #elif defined VISP_HAVE_D3D9
249  m_displayInt = new vpDisplayD3D;
250 #elif defined VISP_HAVE_GTK
251  m_displayInt = new vpDisplayGTK;
252 #endif
253  m_displayInt->init(m_Iint, 50, 50, "Visual servoing with moments");
254 #endif
255 
256  paramRobot(); // set up robot parameters
257 
258  m_task.setServo(vpServo::EYEINHAND_CAMERA);
259  initScene(); // initialize graphical scene (for interface)
260  initFeatures(); // initialize moment features
261  }
262 
263  // launch the simulation
264  void execute(unsigned int nbIter)
265  {
266  vpPlot ViSP_plot;
267  init_visp_plot(ViSP_plot); // Initialize plot object
268 
269  // init main object: using moments up to order 6
270  vpMomentObject obj(6);
271  // setting object type (disrete, continuous[form polygon])
273 
274  std::cout << "Display task information " << std::endl;
275  m_task.print();
276 
277  vpDisplay::display(m_Iint);
278  vpDisplay::flush(m_Iint);
279  unsigned int iter = 0;
280 
281  vpHomogeneousMatrix wMo; // Set to identity
282  vpHomogeneousMatrix wMc; // Camera position in the world frame
283  wMc = wMo * m_cMo.inverse();
284  m_robot.setPosition(wMc);
285  double sampling_time = 0.010; // Sampling period in seconds
286  m_robot.setSamplingTime(sampling_time);
287 
289  while (iter++ < nbIter) {
290 
291  vpColVector v;
292  double t = vpTime::measureTimeMs();
293  // get the cMo
294  wMc = m_robot.getPosition();
295  m_cMo = wMc.inverse() * wMo;
296  // setup the plane in A,B,C style
297  vpPlane pl;
298  double A, B, C;
299  pl.setABCD(0, 0, 1.0, 0);
300  pl.changeFrame(m_cMo);
301  planeToABC(pl, A, B, C);
302 
303  // track points, draw points and add refresh our object
304  refreshScene(obj);
305  // this is the most important thing to do: update our moments
306  m_moments->updateAll(obj);
307  // and update our features. Do it in that order. Features need to use the
308  // information computed by moments
309  m_featureMoments->updateAll(A, B, C);
310  // some graphics again
311  m_imsim.setCameraPosition(m_cMo);
312 
313  m_Iint = m_start_img;
314 
315  m_imsim.getImage(m_Iint, m_cam);
316  vpDisplay::display(m_Iint);
317 
318  if (iter == 1) {
319  vpDisplay::displayText(m_Iint, 20, 20, "Click to start servoing", vpColor::red);
320  vpDisplay::flush(m_Iint);
321  vpDisplay::getClick(m_Iint);
322  }
323  v = m_task.computeControlLaw();
324 
325  std::cout << " || s - s* || = " << m_task.error.sumSquare() << std::endl;
326 
327  m_robot.setVelocity(vpRobot::CAMERA_FRAME, v);
328 
329  ViSP_plot.plot(0, iter, v);
330  ViSP_plot.plot(1, iter, vpPoseVector(m_cMo)); // Plot the velocities
331  ViSP_plot.plot(2, iter, m_task.getError()); // cMo as translations and theta_u
332 
333  m_error = (m_task.getError()).sumSquare();
334 
335 #if defined(PRINT_CONDITION_NUMBER)
336  /*
337  * Condition number of interaction matrix
338  */
339  vpMatrix Linteraction = m_task.L;
340  vpMatrix tmpry, U;
341  vpColVector singularvals;
342  Linteraction.svd(singularvals, tmpry);
343  double condno = static_cast<double>(singularvals.getMaxValue() / singularvals.getMinValue());
344  std::cout << "Condition Number: " << condno << std::endl;
345 #endif
346  vpDisplay::displayText(m_Iint, 20, 20, "Click to stop visual servo...", vpColor::red);
347  if (vpDisplay::getClick(m_Iint, false)) {
348  break;
349  }
350  vpDisplay::flush(m_Iint);
351  vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
352  }
353 
354  m_imsim.getImage(m_Iint, m_cam);
355  vpDisplay::display(m_Iint);
356  vpDisplay::displayText(m_Iint, 20, 20, "Click to quit...", vpColor::red);
357  vpDisplay::flush(m_Iint);
358  vpDisplay::getClick(m_Iint);
359  }
360 
361  void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { m_interaction_type = type; }
362 
363  double error() { return m_error; }
364 
365  void planeToABC(vpPlane &pl, double &A, double &B, double &C)
366  {
367  if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
368  std::cout << "Invalid position:" << std::endl;
369  std::cout << m_cMo << std::endl;
370  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
371  throw vpException(vpException::divideByZeroError, "invalid position!");
372  }
373  A = -pl.getA() / pl.getD();
374  B = -pl.getB() / pl.getD();
375  C = -pl.getC() / pl.getD();
376  }
377 
378  void init_visp_plot(vpPlot &ViSP_plot)
379  {
380  /* -------------------------------------
381  * Initialize ViSP Plotting
382  * -------------------------------------
383  */
384  const unsigned int NbGraphs = 3; // No. of graphs
385  const unsigned int NbCurves_in_graph[NbGraphs] = {6, 6, 6}; // Curves in each graph
386 
387  ViSP_plot.init(NbGraphs, 800, 800, 100 + static_cast<int>(m_width), 50, "Visual Servoing results...");
388 
389  vpColor Colors[6] = {// Colour for s1, s2, s3, in 1st plot
391 
392  for (unsigned int p = 0; p < NbGraphs; p++) {
393  ViSP_plot.initGraph(p, NbCurves_in_graph[p]);
394  for (unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
395  ViSP_plot.setColor(p, c, Colors[c]);
396  }
397 
398  ViSP_plot.setTitle(0, "Robot velocities");
399  ViSP_plot.setLegend(0, 0, "v_x");
400  ViSP_plot.setLegend(0, 1, "v_y");
401  ViSP_plot.setLegend(0, 2, "v_z");
402  ViSP_plot.setLegend(0, 3, "w_x");
403  ViSP_plot.setLegend(0, 4, "w_y");
404  ViSP_plot.setLegend(0, 5, "w_z");
405 
406  ViSP_plot.setTitle(1, "Camera pose cMo");
407  ViSP_plot.setLegend(1, 0, "tx");
408  ViSP_plot.setLegend(1, 1, "ty");
409  ViSP_plot.setLegend(1, 2, "tz");
410  ViSP_plot.setLegend(1, 3, "tu_x");
411  ViSP_plot.setLegend(1, 4, "tu_y");
412  ViSP_plot.setLegend(1, 5, "tu_z");
413 
414  ViSP_plot.setTitle(2, "Error in visual features: ");
415  ViSP_plot.setLegend(2, 0, "x_n");
416  ViSP_plot.setLegend(2, 1, "y_n");
417  ViSP_plot.setLegend(2, 2, "a_n");
418  ViSP_plot.setLegend(2, 3, "sx");
419  ViSP_plot.setLegend(2, 4, "sy");
420  ViSP_plot.setLegend(2, 5, "alpha");
421  }
422 
423 protected:
424  // start and destination positioning matrices
425  unsigned int m_width;
426  unsigned int m_height;
427 
428  // start and destination positioning matrices
429  vpHomogeneousMatrix m_cMo;
430  vpHomogeneousMatrix m_cdMo;
431 
432  vpSimulatorCamera m_robot; // robot used in this simulation
433  vpImage<vpRGBa> m_Iint; // internal image used for interface display
434  vpServo m_task; // servoing task
435  vpCameraParameters m_cam; // robot camera parameters
436  double m_error; // current error
437  vpImageSimulator m_imsim; // image simulator used to simulate the perspective-projection camera
438 
439  // several images used in the simulation
440  vpImage<unsigned char> m_cur_img;
441  vpImage<unsigned char> m_src_img;
442  vpImage<unsigned char> m_dst_img;
443  vpImage<vpRGBa> m_start_img;
444  vpServo::vpServoIteractionMatrixType m_interaction_type; // current or desired
445  // source and destination objects for moment manipulation
446  vpMomentObject m_src;
447  vpMomentObject m_dst;
448 
449  // moment sets and their corresponding features
450  vpMomentCommon *m_moments;
451  vpMomentCommon *m_momentsDes;
452  vpFeatureMomentCommon *m_featureMoments;
453  vpFeatureMomentCommon *m_featureMomentsDes;
454 
455  vpDisplay *m_displayInt;
456 };
457 #endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
458 
459 int main()
460 {
461  try {
462  // intial pose
463  vpHomogeneousMatrix cMo(-0.1, -0.1, 1.5, -vpMath::rad(20), -vpMath::rad(20), -vpMath::rad(30));
464  // Desired pose
466 
467  servoMoment servo;
468  // init the simulation
469  servo.init(cMo, cdMo);
470 
471  servo.execute(1500);
472  return EXIT_SUCCESS;
473  } catch (const vpException &e) {
474  std::cout << "Catch an exception: " << e << std::endl;
475  return EXIT_FAILURE;
476  }
477 }
478 
479 #endif
Type getMinValue() const
Definition: vpArray2D.h:895
Type getMaxValue() const
Definition: vpArray2D.h:912
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
double sumSquare() const
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
static const vpColor red
Definition: vpColor.h:217
static const vpColor cyan
Definition: vpColor.h:226
static const vpColor orange
Definition: vpColor.h:227
static const vpColor blue
Definition: vpColor.h:223
static const vpColor purple
Definition: vpColor.h:228
static const vpColor green
Definition: vpColor.h:220
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Definition: vpDisplayD3D.h:107
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:135
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
Class that defines generic functionnalities for display.
Definition: vpDisplay.h:178
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
This class allows to access common vpFeatureMoments in a pre-filled database.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class which enables to project an image in the 3D space and get the view of a virtual camera.
void getImage(vpImage< unsigned char > &I, const vpCameraParameters &cam)
void init(const vpImage< unsigned char > &I, vpColVector *X)
void setInterpolationType(const vpInterpolationType interplt)
void setCameraPosition(const vpHomogeneousMatrix &cMt)
static double rad(double deg)
Definition: vpMath.h:110
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:2030
This class initializes and allows access to commonly used moments.
static std::vector< double > getMu3(vpMomentObject &object)
static double getAlpha(vpMomentObject &object)
static double getSurface(vpMomentObject &object)
Class for generic objects.
void setType(vpObjectType input_type)
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:59
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
double getD() const
Definition: vpPlane.h:108
double getA() const
Definition: vpPlane.h:102
double getC() const
Definition: vpPlane.h:106
void setABCD(double a, double b, double c, double d)
Definition: vpPlane.h:90
double getB() const
Definition: vpPlane.h:104
This class enables real time drawing of 2D or 3D graphics. An instance of the class open a window whi...
Definition: vpPlot.h:116
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:206
void init(unsigned int nbGraph, unsigned int height=700, unsigned int width=700, int x=-1, int y=-1, const std::string &title="")
Definition: vpPlot.cpp:100
void setLegend(unsigned int graphNum, unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
void plot(unsigned int graphNum, unsigned int curveNum, double x, double y)
Definition: vpPlot.cpp:286
void setColor(unsigned int graphNum, unsigned int curveNum, vpColor color)
Definition: vpPlot.cpp:261
void setTitle(unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Definition: vpRGBa.h:67
@ CAMERA_FRAME
Definition: vpRobot.h:82
@ EYEINHAND_CAMERA
Definition: vpServo.h:155
vpServoIteractionMatrixType
Definition: vpServo.h:181
@ CURRENT
Definition: vpServo.h:182
Class that defines the simplest robot: a free flying camera.
Class that consider the case of a translation vector.
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()