Visual Servoing Platform version 3.5.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
71int 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)
82int 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
93class servoMoment
94{
95public:
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
166 m_src.fromImage(m_src_img, 128, m_cam);
167
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
423protected:
424 // start and destination positioning matrices
425 unsigned int m_width;
426 unsigned int m_height;
427
428 // start and destination positioning matrices
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
459int 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:135
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()