Visual Servoing Platform  version 3.1.0
servoMomentImage.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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/vpConfig.h>
50 #include <visp3/core/vpDebug.h>
51 #include <visp3/core/vpHomogeneousMatrix.h>
52 #include <visp3/core/vpIoTools.h>
53 #include <visp3/core/vpMath.h>
54 #include <visp3/core/vpMomentCommon.h>
55 #include <visp3/core/vpMomentDatabase.h>
56 #include <visp3/core/vpMomentObject.h>
57 #include <visp3/core/vpPlane.h>
58 #include <visp3/core/vpPoseVector.h>
59 #include <visp3/gui/vpDisplayGDI.h>
60 #include <visp3/gui/vpDisplayGTK.h>
61 #include <visp3/gui/vpDisplayOpenCV.h>
62 #include <visp3/gui/vpDisplayX.h>
63 #include <visp3/gui/vpPlot.h>
64 #include <visp3/robot/vpImageSimulator.h>
65 #include <visp3/robot/vpSimulatorCamera.h>
66 #include <visp3/visual_features/vpFeatureBuilder.h>
67 #include <visp3/visual_features/vpFeatureMomentCommon.h>
68 #include <visp3/visual_features/vpFeaturePoint.h>
69 #include <visp3/vs/vpServo.h>
70 
71 #if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
72 // Robot simulator used in this example is not available
73 int main()
74 {
75  std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
76  "not available."
77  << std::endl;
78  std::cout << "You should install pthread third-party library." << std::endl;
79 }
80 // No display available
81 #elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_OPENCV) && !defined(VISP_HAVE_GDI) && !defined(VISP_HAVE_D3D9) && \
82  !defined(VISP_HAVE_GTK)
83 int main()
84 {
85  std::cout << "Can't run this example since no display capability is available." << std::endl;
86  std::cout << "You should install one of the following third-party library: "
87  "X11, OpenCV, GDI, GTK."
88  << 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  } catch (vpException &e) {
125  std::cout << "Catch an exception: " << e << std::endl;
126  return 1;
127  }
128 }
129 
130 // init the right display
131 #if defined VISP_HAVE_X11
132 vpDisplayX displayInt;
133 #elif defined VISP_HAVE_OPENCV
134 vpDisplayOpenCV displayInt;
135 #elif defined VISP_HAVE_GDI
136 vpDisplayGDI displayInt;
137 #elif defined VISP_HAVE_D3D9
138 vpDisplayD3D displayInt;
139 #elif defined VISP_HAVE_GTK
140 vpDisplayGTK displayInt;
141 #endif
142 
143 // start and destination positioning matrices
146 
147 vpSimulatorCamera robot; // robot used in this simulation
148 vpImage<vpRGBa> Iint(480, 640, 0); // internal image used for interface
149  // display
150 vpServo task; // servoing task
151 vpCameraParameters cam; // robot camera parameters
152 double _error; // current error
153 vpImageSimulator imsim; // image simulator used to simulate the
154  // 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 void initScene()
175 {
176  vpColVector X[4];
177  for (int i = 0; i < 4; i++)
178  X[i].resize(3);
179  X[0][0] = -0.2;
180  X[0][1] = -0.1;
181  X[0][2] = 0;
182 
183  X[1][0] = 0.2;
184  X[1][1] = -0.1;
185  X[1][2] = 0;
186 
187  X[2][0] = 0.2;
188  X[2][1] = 0.1;
189  X[2][2] = 0;
190 
191  X[3][0] = -0.2;
192  X[3][1] = 0.1;
193  X[3][2] = 0;
194  // init source and destination images
195  vpImage<unsigned char> tmp_img(480, 640, 255);
196  vpImage<vpRGBa> tmp_start_img(480, 640, vpRGBa(255, 0, 0));
197 
198  vpImageSimulator imsim_start;
200  imsim_start.init(tmp_start_img, X);
201  imsim_start.setCameraPosition(cdMo);
202  imsim_start.getImage(start_img, cam);
203 
205  imsim.init(tmp_img, X);
206 
207  imsim.setCameraPosition(cMo);
208  imsim.getImage(src_img, cam);
209 
211  src.fromImage(src_img, 128, cam);
212 
214  imsim.setCameraPosition(cdMo);
215  imsim.getImage(dst_img, cam);
216  dst.fromImage(dst_img, 128, cam);
217 }
218 
219 void refreshScene(vpMomentObject &obj)
220 {
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 {
245  // A,B,C parameters of source and destination plane
246  double A;
247  double B;
248  double C;
249  double Ad;
250  double Bd;
251  double Cd;
252  // init main object: using moments up to order 5
253 
254  // Initializing values from regular plane (with ax+by+cz=d convention)
255  vpPlane pl;
256  pl.setABCD(0, 0, 1.0, 0);
257  pl.changeFrame(cMo);
258  planeToABC(pl, A, B, C);
259 
260  pl.setABCD(0, 0, 1.0, 0);
261  pl.changeFrame(cdMo);
262  planeToABC(pl, Ad, Bd, Cd);
263 
264  // extracting initial position (actually we only care about Zdst)
266  cdMo.extract(vec);
267 
270  // don't need to be specific, vpMomentCommon automatically loads
271  // Xg,Yg,An,Ci,Cj,Alpha moments
273  vpMomentCommon::getAlpha(dst), vec[2], true);
275  vpMomentCommon::getAlpha(dst), vec[2], true);
276  // same thing with common features
277  featureMoments = new vpFeatureMomentCommon(*moments);
278  featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
279 
280  moments->updateAll(src);
281  momentsDes->updateAll(dst);
282 
283  featureMoments->updateAll(A, B, C);
284  featureMomentsDes->updateAll(Ad, Bd, Cd);
285 
286  // setup the interaction type
287  task.setInteractionMatrixType(interaction_type);
290  task.addFeature(featureMoments->getFeatureGravityNormalized(), featureMomentsDes->getFeatureGravityNormalized());
291  task.addFeature(featureMoments->getFeatureAn(), featureMomentsDes->getFeatureAn());
292  // the moments are different in case of a symmetric object
293  task.addFeature(featureMoments->getFeatureCInvariant(), featureMomentsDes->getFeatureCInvariant(),
294  (1 << 10) | (1 << 11));
295  task.addFeature(featureMoments->getFeatureAlpha(), featureMomentsDes->getFeatureAlpha());
296 
297  task.setLambda(1.);
298 }
299 
300 void execute(unsigned int nbIter)
301 {
302 
303  vpPlot ViSP_plot;
304  init_visp_plot(ViSP_plot); // Initialize plot object
305 
306  // init main object: using moments up to order 6
307  vpMomentObject obj(6);
308  // setting object type (disrete, continuous[form polygon])
310 
311  vpTRACE("Display task information ");
312  task.print();
313 
314  vpDisplay::display(Iint);
315  vpDisplay::flush(Iint);
316  unsigned int iter = 0;
317 
318  vpHomogeneousMatrix wMo; // Set to identity
319  vpHomogeneousMatrix wMc; // Camera position in the world frame
320  wMc = wMo * cMo.inverse();
321  robot.setPosition(wMc);
322  float sampling_time = 0.010f; // Sampling period in seconds
323  robot.setSamplingTime(sampling_time);
324 
325  // For plotting
326  vpPoseVector currentpose;
327  vpColVector err_features;
328 
330  while (iter++ < nbIter) {
331 
332  vpColVector v;
333  double t = vpTime::measureTimeMs();
334  // get the cMo
335  wMc = robot.getPosition();
336  cMo = wMc.inverse() * wMo;
337  currentpose.buildFrom(cMo); // For plot
338  // setup the plane in A,B,C style
339  vpPlane pl;
340  double A, B, C;
341  pl.setABCD(0, 0, 1.0, 0);
342  pl.changeFrame(cMo);
343  planeToABC(pl, A, B, C);
344 
345  // track points, draw points and add refresh our object
346  refreshScene(obj);
347  // this is the most important thing to do: update our moments
348  moments->updateAll(obj);
349  // and update our features. Do it in that order. Features need to use the
350  // information computed by moments
351  featureMoments->updateAll(A, B, C);
352  // some graphics again
353  imsim.setCameraPosition(cMo);
354 
355  Iint = start_img;
356 
357  imsim.getImage(Iint, cam);
358  vpDisplay::display(Iint);
359 
360  vpDisplay::flush(Iint);
361 
362  if (iter == 1)
363  vpDisplay::getClick(Iint);
364  v = task.computeControlLaw();
365  // pilot robot using position control. The displacement is t*v with t=10ms
366  // step robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
367 
368  err_features = task.error;
369  std::cout << " || s - s* || = " << task.error.sumSquare() << std::endl;
370 
372  vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
373 
374  ViSP_plot.plot(0, iter, v);
375  ViSP_plot.plot(1, iter, currentpose); // Plot the velocities
376  ViSP_plot.plot(2, iter, err_features); // cMo as translations and theta_u
377 
378  _error = (task.getError()).sumSquare();
379 
380 #if defined(PRINT_CONDITION_NUMBER)
381  /*
382  * Condition number of interaction matrix
383  */
384  vpMatrix Linteraction = task.L;
385  vpMatrix tmpry, U;
386  vpColVector singularvals;
387  Linteraction.svd(singularvals, tmpry);
388  double condno = static_cast<double>(singularvals.getMaxValue() / singularvals.getMinValue());
389  std::cout << "Condition Number: " << condno << std::endl;
390 #endif
391  }
392 
393  task.kill();
394 
395  vpTRACE("\n\nClick in the internal view window to end...");
396  vpDisplay::getClick(Iint);
397 
398  delete moments;
399  delete momentsDes;
400  delete featureMoments;
401  delete featureMomentsDes;
402 }
403 
404 void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { interaction_type = type; }
405 double error() { return _error; }
406 
407 void planeToABC(vpPlane &pl, double &A, double &B, double &C)
408 {
409  if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
410  std::cout << "Invalid position:" << std::endl;
411  std::cout << cMo << std::endl;
412  std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
413  throw vpException(vpException::divideByZeroError, "invalid position!");
414  }
415  A = -pl.getA() / pl.getD();
416  B = -pl.getB() / pl.getD();
417  C = -pl.getC() / pl.getD();
418 }
419 
420 void paramRobot() { cam = vpCameraParameters(640, 480, 320, 240); }
421 
422 void init_visp_plot(vpPlot &ViSP_plot)
423 {
424  /* -------------------------------------
425  * Initialize ViSP Plotting
426  * -------------------------------------
427  */
428  const unsigned int NbGraphs = 3; // No. of graphs
429  const unsigned int NbCurves_in_graph[NbGraphs] = {6, 6, 6}; // Curves in each graph
430 
431  ViSP_plot.init(NbGraphs, 800, 800, 10, 10, "Visual Servoing results...");
432 
433  vpColor Colors[6] = {// Colour for s1, s2, s3, in 1st plot
435 
436  for (unsigned int p = 0; p < NbGraphs; p++) {
437  ViSP_plot.initGraph(p, NbCurves_in_graph[p]);
438  for (unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
439  ViSP_plot.setColor(p, c, Colors[c]);
440  }
441 
442  ViSP_plot.setTitle(0, "Robot velocities");
443  ViSP_plot.setLegend(0, 0, "v_x");
444  ViSP_plot.setLegend(0, 1, "v_y");
445  ViSP_plot.setLegend(0, 2, "v_z");
446  ViSP_plot.setLegend(0, 3, "w_x");
447  ViSP_plot.setLegend(0, 4, "w_y");
448  ViSP_plot.setLegend(0, 5, "w_z");
449 
450  ViSP_plot.setTitle(1, "Camera pose cMo");
451  ViSP_plot.setLegend(1, 0, "tx");
452  ViSP_plot.setLegend(1, 1, "ty");
453  ViSP_plot.setLegend(1, 2, "tz");
454  ViSP_plot.setLegend(1, 3, "tu_x");
455  ViSP_plot.setLegend(1, 4, "tu_y");
456  ViSP_plot.setLegend(1, 5, "tu_z");
457 
458  ViSP_plot.setTitle(2, "Error in visual features: ");
459  ViSP_plot.setLegend(2, 0, "x_n");
460  ViSP_plot.setLegend(2, 1, "y_n");
461  ViSP_plot.setLegend(2, 2, "a_n");
462  ViSP_plot.setLegend(2, 3, "sx");
463  ViSP_plot.setLegend(2, 4, "sy");
464  ViSP_plot.setLegend(2, 5, "alpha");
465 }
466 #endif
void setPosition(const vpHomogeneousMatrix &wMc)
void svd(vpColVector &w, vpMatrix &V)
Definition: vpMatrix.cpp:1791
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
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:261
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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:129
Class to define colors available for display functionnalities.
Definition: vpColor.h:120
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:497
This class allows to access common vpFeatureMoments in a pre-filled database.
error that can be emited by ViSP classes.
Definition: vpException.h:71
void setABCD(const double a, const double b, const double c, const double d)
Definition: vpPlane.h:90
Class for generic objects.
vpHomogeneousMatrix inverse() const
void plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
Definition: vpPlot.cpp:286
vpHomogeneousMatrix getPosition() const
void extract(vpRotationMatrix &R) const
static const vpColor green
Definition: vpColor.h:183
static void flush(const vpImage< unsigned char > &I)
void setTitle(const unsigned int graphNum, const std::string &title)
Definition: vpPlot.cpp:498
VISP_EXPORT double measureTimeMs()
Definition: vpTime.cpp:88
Definition: vpRGBa.h:66
static const vpColor red
Definition: vpColor.h:180
void fromImage(const vpImage< unsigned char > &image, unsigned char threshold, const vpCameraParameters &cam)
static const vpColor orange
Definition: vpColor.h:190
Type getMaxValue() const
Definition: vpArray2D.h:671
virtual void setSamplingTime(const double &delta_t)
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed...
Definition: vpDisplayD3D.h:107
void kill()
Definition: vpServo.cpp:192
void setCameraPosition(const vpHomogeneousMatrix &cMt)
static const vpColor cyan
Definition: vpColor.h:189
Type getMinValue() const
Definition: vpArray2D.h:655
double getD() const
Definition: vpPlane.h:108
vpColVector computeControlLaw()
Definition: vpServo.cpp:935
vpFeatureMomentAlpha & getFeatureAlpha()
void updateAll(double A, double B, double C)
void setInterpolationType(const vpInterpolationType interplt)
void changeFrame(const vpHomogeneousMatrix &cMo)
Definition: vpPlane.cpp:354
#define vpTRACE
Definition: vpDebug.h:416
static std::vector< double > getMu3(vpMomentObject &object)
vpMatrix L
Interaction matrix.
Definition: vpServo.h:544
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
double getB() const
Definition: vpPlane.h:104
Generic class defining intrinsic camera parameters.
void setLambda(double c)
Definition: vpServo.h:406
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 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
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 std::string &title="")
Definition: vpPlot.cpp:100
vpServoIteractionMatrixType
Definition: vpServo.h:185
static double getSurface(vpMomentObject &object)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:574
vpFeatureMomentCInvariant & getFeatureCInvariant()
static double rad(double deg)
Definition: vpMath.h:102
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void setLegend(const unsigned int graphNum, const unsigned int curveNum, const std::string &legend)
Definition: vpPlot.cpp:547
double getA() const
Definition: vpPlane.h:102
void updateAll(vpMomentObject &object)
void initGraph(unsigned int graphNum, unsigned int curveNbr)
Definition: vpPlot.cpp:206
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:92
vpFeatureMomentGravityCenterNormalized & getFeatureGravityNormalized()
double sumSquare() const
void setType(vpObjectType input_type)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
Definition: vpServo.cpp:313
double getC() 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:115
vpColVector getError() const
Definition: vpServo.h:282
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
This class defines the container for a plane geometrical structure.
Definition: vpPlane.h:58
static const vpColor purple
Definition: vpColor.h:191
vpFeatureMomentAreaNormalized & getFeatureAn()
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:223
vpColVector error
Definition: vpServo.h:549
Class that consider the case of a translation vector.
static const vpColor blue
Definition: vpColor.h:186