 |
Visual Servoing Platform
version 3.3.0
|
56 #include <visp3/core/vpConfig.h>
58 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
63 #include <visp3/core/vpCameraParameters.h>
64 #include <visp3/core/vpCylinder.h>
65 #include <visp3/core/vpHomogeneousMatrix.h>
66 #include <visp3/core/vpImage.h>
67 #include <visp3/core/vpMath.h>
68 #include <visp3/gui/vpDisplayGDI.h>
69 #include <visp3/gui/vpDisplayGTK.h>
70 #include <visp3/gui/vpDisplayOpenCV.h>
71 #include <visp3/gui/vpDisplayX.h>
72 #include <visp3/gui/vpProjectionDisplay.h>
73 #include <visp3/io/vpParseArgv.h>
74 #include <visp3/robot/vpSimulatorCamera.h>
75 #include <visp3/visual_features/vpFeatureBuilder.h>
76 #include <visp3/visual_features/vpFeatureLine.h>
77 #include <visp3/vs/vpServo.h>
78 #include <visp3/vs/vpServoDisplay.h>
81 #define GETOPTARGS "cdh"
83 void usage(
const char *name,
const char *badparam);
84 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
94 void usage(
const char *name,
const char *badparam)
97 Simulation of a 2D visual servoing on a cylinder:\n\
98 - eye-in-hand control law,\n\
99 - velocity computed in the camera frame,\n\
100 - display the camera view.\n\
103 %s [-c] [-d] [-h]\n", name);
109 Disable the mouse click. Useful to automaze the \n\
110 execution of this program without humain intervention.\n\
113 Turn off the display.\n\
119 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
133 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
141 click_allowed =
false;
147 usage(argv[0], NULL);
152 usage(argv[0], optarg_);
158 if ((c == 1) || (c == -1)) {
160 usage(argv[0], NULL);
161 std::cerr <<
"ERROR: " << std::endl;
162 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
169 int main(
int argc,
const char **argv)
172 bool opt_display =
true;
173 bool opt_click_allowed =
true;
176 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
184 #if defined VISP_HAVE_X11
187 #elif defined VISP_HAVE_GTK
190 #elif defined VISP_HAVE_GDI
193 #elif defined VISP_HAVE_OPENCV
202 displayInt.
init(Iint, 100, 100,
"Internal view");
203 displayExt.
init(Iext, (
int)(130 + Iint.
getWidth()), 100,
"External view");
247 externalview.
insert(cylinder);
250 cylinder.track(cMod);
257 for (i = 0; i < 2; i++)
268 for (i = 0; i < 2; i++) {
302 if (opt_display && opt_click_allowed) {
303 std::cout <<
"\n\nClick in the internal camera view window to start..." << std::endl;
313 unsigned int iter = 0;
316 std::cout <<
"---------------------------------------------" << iter++ << std::endl;
332 for (i = 0; i < 2; i++) {
352 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
353 }
while ((task.
getError()).sumSquare() > 1e-9);
373 double vitesse = 0.5;
374 unsigned int tempo = 800;
376 while (iter < tempo) {
385 for (i = 0; i < 2; i++) {
400 if (iter % tempo < 200 /*&& iter%tempo >= 0*/) {
402 e1[0] = fabs(vitesse);
404 rapport = vitesse / proj_e1[0];
409 if (iter % tempo < 400 && iter % tempo >= 200) {
411 e2[1] = fabs(vitesse);
413 rapport = vitesse / proj_e2[1];
418 if (iter % tempo < 600 && iter % tempo >= 400) {
420 e1[0] = -fabs(vitesse);
422 rapport = -vitesse / proj_e1[0];
427 if (iter % tempo < 800 && iter % tempo >= 600) {
429 e2[1] = -fabs(vitesse);
431 rapport = -vitesse / proj_e2[1];
438 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
443 if (opt_display && opt_click_allowed) {
444 std::cout <<
"\nClick in the internal camera view window to end..." << std::endl;
453 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
461 std::cout <<
"You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..." << std::endl;
462 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
463 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
464 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
465 std::cout <<
"- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
static double rad(double deg)
Generic class defining intrinsic camera parameters.
Display for windows using GDI (available on any windows 32 platform).
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines the simplest robot: a free flying camera.
Implementation of column vector and the associated operations.
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
void setServo(const vpServoType &servo_type)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines what is a cylinder.
vpHomogeneousMatrix getPosition() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void display(const vpImage< unsigned char > &I)
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 winx=-1, int winy=-1, const std::string &title="")
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
vpColVector getError() const
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, unsigned int thickness=1)
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void print(unsigned int select=FEATURE_ALL) const
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpColVector computeControlLaw()
static void flush(const vpImage< unsigned char > &I)
interface with the image for feature display
vpHomogeneousMatrix inverse() const
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void insert(vpForwardProjection &fp)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
error that can be emited by ViSP classes.
unsigned int getWidth() const