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