53 #include <visp/vpDebug.h>
54 #include <visp/vpConfig.h>
56 #if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
61 #include <visp/vpCameraParameters.h>
62 #include <visp/vpDisplayX.h>
63 #include <visp/vpDisplayGTK.h>
64 #include <visp/vpDisplayGDI.h>
65 #include <visp/vpFeatureBuilder.h>
66 #include <visp/vpFeatureLine.h>
67 #include <visp/vpHomogeneousMatrix.h>
68 #include <visp/vpImage.h>
69 #include <visp/vpLine.h>
70 #include <visp/vpMath.h>
71 #include <visp/vpParseArgv.h>
72 #include <visp/vpRobotCamera.h>
73 #include <visp/vpServo.h>
74 #include <visp/vpServoDisplay.h>
75 #include <visp/vpSimulatorCamera.h>
78 #define GETOPTARGS "cdh"
88 void usage(
const char *name,
const char *badparam)
91 Simulation of 2D a visual servoing on a line:\n\
92 - eye-in-hand control law,\n\
93 - velocity computed in the camera frame,\n\
94 - display the camera view.\n\
97 %s [-c] [-d] [-h]\n", name);
103 Disable the mouse click. Useful to automaze the \n\
104 execution of this program without humain intervention.\n\
107 Turn off the display.\n\
113 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
128 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
135 case 'c': click_allowed =
false;
break;
136 case 'd': display =
false;
break;
137 case 'h': usage(argv[0], NULL);
return false;
break;
140 usage(argv[0], optarg);
145 if ((c == 1) || (c == -1)) {
147 usage(argv[0], NULL);
148 std::cerr <<
"ERROR: " << std::endl;
149 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
158 main(
int argc,
const char ** argv)
160 bool opt_display =
true;
161 bool opt_click_allowed =
true;
164 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
171 #if defined VISP_HAVE_X11
173 #elif defined VISP_HAVE_GTK
175 #elif defined VISP_HAVE_GDI
182 display.
init(I, 100, 100,
"Camera view...") ;
199 double px, py ; px = py = 600 ;
200 double u0, v0 ; u0 = v0 = 256 ;
234 for(
int i = 0; i < nbline; i++)
236 line[i].
track(cMod) ;
244 for(
int i = 0; i < nbline; i++)
263 for(
int i = 0; i < nbline; i++)
276 if (opt_display && opt_click_allowed) {
277 std::cout <<
"\n\nClick in the camera view window to start..." << std::endl;
281 unsigned int iter=0 ;
285 std::cout <<
"---------------------------------------------" << iter <<std::endl ;
294 for(
int i = 0; i < nbline; i++)
312 std::cout <<
"|| s - s* || = " << ( task.
getError() ).sumSquare() <<std::endl ; ;
316 if (opt_display && opt_click_allowed) {
317 std::cout <<
"\nClick in the camera view window to end..." << std::endl;
330 vpERROR_TRACE(
"You do not have X11, GTK or GDI display functionalities...");
static void display(vpServo &s, const vpCameraParameters &cam, vpImage< unsigned char > &I, vpColor currentColor=vpColor::green, vpColor desiredColor=vpColor::red, unsigned int thickness=1)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void print(const unsigned int select=FEATURE_ALL) const
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Class that defines the simplest robot: a free flying camera.
void setWorldCoordinates(const double &A1, const double &B1, const double &C1, const double &D1, const double &A2, const double &B2, const double &C2, const double &D2)
Display for windows using GDI (available on any windows 32 platform).
Define the X11 console to display images.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
create a new ste of two visual features
void setLambda(double _lambda)
set the gain lambda
void track(const vpHomogeneousMatrix &cMo)
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines a line in the object frame, the camera frame and the image plane. All the parameters must be set in meter.
void kill()
destruction (memory deallocation if required)
vpColVector getError() const
virtual void print() const
vpColVector computeControlLaw()
compute the desired control law
static void display(const vpImage< unsigned char > &I)
Generic class defining intrinsic camera parameters.
void getPosition(vpHomogeneousMatrix &wMc) const
Class that defines a 2D line visual feature which is composed by two parameters that are and ...
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const char *title=NULL)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Set the type of the interaction matrix (current, mean, desired, user).
static double rad(double deg)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
vpHomogeneousMatrix inverse() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
virtual bool getClick(bool blocking=true)=0
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class required to compute the visual servoing control law descbribed in and .
void setServo(vpServoType _servo_type)
Choice of the visual servoing control law.