 |
Visual Servoing Platform
version 3.2.0
|
56 #include <visp3/core/vpConfig.h>
57 #include <visp3/core/vpDebug.h>
59 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
64 #include <visp3/core/vpCameraParameters.h>
65 #include <visp3/core/vpHomogeneousMatrix.h>
66 #include <visp3/core/vpImage.h>
67 #include <visp3/core/vpImagePoint.h>
68 #include <visp3/core/vpIoTools.h>
69 #include <visp3/core/vpMath.h>
70 #include <visp3/core/vpMeterPixelConversion.h>
71 #include <visp3/gui/vpDisplayGDI.h>
72 #include <visp3/gui/vpDisplayGTK.h>
73 #include <visp3/gui/vpDisplayOpenCV.h>
74 #include <visp3/gui/vpDisplayX.h>
75 #include <visp3/gui/vpProjectionDisplay.h>
76 #include <visp3/io/vpParseArgv.h>
77 #include <visp3/robot/vpSimulatorCamera.h>
78 #include <visp3/visual_features/vpFeatureBuilder.h>
79 #include <visp3/visual_features/vpFeaturePointPolar.h>
80 #include <visp3/vs/vpServo.h>
81 #include <visp3/vs/vpServoDisplay.h>
84 #define GETOPTARGS "cdh"
86 void usage(
const char *name,
const char *badparam);
87 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
97 void usage(
const char *name,
const char *badparam)
100 Tests a control law with the following characteristics:\n\
101 - eye-in-hand control\n\
102 - articular velocity are computed\n\
103 - servo on 4 points,\n\
104 - internal and external camera view displays.\n\
107 %s [-c] [-d] [-h]\n", name);
112 Disable the mouse click. Useful to automaze the \n\
113 execution of this program without humain intervention.\n\
116 Turn off the display.\n\
122 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
136 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
144 click_allowed =
false;
150 usage(argv[0], NULL);
155 usage(argv[0], optarg_);
161 if ((c == 1) || (c == -1)) {
163 usage(argv[0], NULL);
164 std::cerr <<
"ERROR: " << std::endl;
165 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
172 int main(
int argc,
const char **argv)
181 std::string username;
186 std::string logdirname;
188 logdirname =
"C:/temp/" + username;
190 logdirname =
"/tmp/" + username;
199 std::cerr << std::endl <<
"ERROR:" << std::endl;
200 std::cerr <<
" Cannot create " << logdirname << std::endl;
204 std::string logfilename;
205 logfilename = logdirname +
"/log.dat";
208 std::ofstream flog(logfilename.c_str());
210 bool opt_click_allowed =
true;
211 bool opt_display =
true;
214 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
220 #if defined VISP_HAVE_X11
223 #elif defined VISP_HAVE_GTK
226 #elif defined VISP_HAVE_GDI
229 #elif defined VISP_HAVE_OPENCV
240 displayInt.
init(Iint, 0, 0,
"Internal view");
241 displayExt.
init(Iext, 330, 000,
"External view");
256 std::cout << std::endl;
257 std::cout <<
"----------------------------------------------" << std::endl;
258 std::cout <<
" Test program for vpServo " << std::endl;
259 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
260 std::cout <<
" Simulation " << std::endl;
261 std::cout <<
" task : servo 4 points " << std::endl;
262 std::cout <<
"----------------------------------------------" << std::endl;
263 std::cout << std::endl;
272 #if defined(TRANS_Z_PURE)
277 #elif defined(TRANS_X_PURE)
283 #elif defined(ROT_Z_PURE)
289 #elif defined(ROT_X_PURE)
295 #elif defined(COMPLEX)
301 #elif defined(PROBLEM)
323 for (i = 0; i < 4; i++)
324 externalview.
insert(point[i]);
331 for (i = 0; i < 4; i++) {
332 point[i].
track(cMod);
340 for (i = 0; i < 4; i++)
345 for (i = 0; i < 4; i++) {
370 for (i = 0; i < 4; i++)
376 std::cout <<
"\nDisplay task information: " << std::endl;
379 unsigned int iter = 0;
381 while (iter++ < 200) {
382 std::cout <<
"---------------------------------------------" << iter << std::endl;
396 for (i = 0; i < 4; i++) {
416 std::cout <<
"Display task information: " << std::endl;
428 flog << v[0] <<
" " << v[1] <<
" " << v[2] <<
" " << v[3] <<
" " << v[4] <<
" " << v[5] <<
" ";
430 std::cout <<
"v: " << v.
t() << std::endl;
432 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
437 flog << (task.
getError()).t() <<
" ";
438 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
441 for (i = 0; i < 4; i++) {
445 for (i = 0; i < 4; i++) {
446 flog << point[i].
get_x() <<
" " << point[i].
get_y() <<
" ";
455 std::cout <<
"\nClick in the internal camera view to continue..." << std::endl;
470 std::cout <<
"Final robot position with respect to the object frame:\n";
473 if (opt_display && opt_click_allowed) {
475 std::cout <<
"\n\nClick in the internal view to end..." << std::endl;
480 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
487 std::cout <<
"You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..." << std::endl;
488 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
489 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
490 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
491 std::cout <<
"- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
void set_i(const double ii)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void setWorldCoordinates(const double oX, const double oY, const double oZ)
static double rad(double deg)
Generic class defining intrinsic camera parameters.
void print() const
Print the matrix as a pose vector .
void set_eJe(const vpMatrix &eJe_)
void set_j(const double jj)
double get_x() const
Get the point x coordinate in the image plane.
Class that defines 2D image point visual feature with polar coordinates described in .
Display for windows using GDI (available on any windows 32 platform).
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void set_cVe(const vpVelocityTwistMatrix &cVe_)
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...
Implementation of a matrix and operations on matrices.
void setServo(const vpServoType &servo_type)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
vpHomogeneousMatrix getPosition() const
static const vpColor green
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...
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
vpColVector getError() const
void display(vpImage< unsigned char > &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj=false, const unsigned int thickness=1)
void get_eJe(vpMatrix &eJe)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
vpColVector computeControlLaw()
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, const unsigned int select=vpBasicFeature::FEATURE_ALL)
static void flush(const vpImage< unsigned char > &I)
interface with the image for feature display
vpHomogeneousMatrix inverse() const
Class that defines what is a point.
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.
void track(const vpHomogeneousMatrix &cMo)
double get_y() const
Get the point y coordinate in the image plane.