58 #include <visp3/core/vpConfig.h> 60 #if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) \ 61 && (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)) 66 #include <visp3/core/vpCameraParameters.h> 67 #include <visp3/core/vpHomogeneousMatrix.h> 68 #include <visp3/core/vpImage.h> 69 #include <visp3/core/vpMath.h> 70 #include <visp3/gui/vpDisplayGDI.h> 71 #include <visp3/gui/vpDisplayGTK.h> 72 #include <visp3/gui/vpDisplayOpenCV.h> 73 #include <visp3/gui/vpDisplayX.h> 74 #include <visp3/gui/vpProjectionDisplay.h> 75 #include <visp3/io/vpParseArgv.h> 76 #include <visp3/robot/vpSimulatorCamera.h> 77 #include <visp3/visual_features/vpFeatureBuilder.h> 78 #include <visp3/visual_features/vpFeaturePoint.h> 79 #include <visp3/vs/vpServo.h> 80 #include <visp3/vs/vpServoDisplay.h> 83 #define GETOPTARGS "cdh" 85 void usage(
const char *name,
const char *badparam);
86 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
96 void usage(
const char *name,
const char *badparam)
99 Tests a control law with the following characteristics:\n\ 100 - eye-in-hand control\n\ 101 - articular velocity are computed\n\ 102 - servo on 4 points,\n\ 103 - internal and external camera view displays.\n\ 106 %s [-c] [-d] [-h]\n", name);
111 Disable the mouse click. Useful to automaze the \n\ 112 execution of this program without humain intervention.\n\ 115 Turn off the display.\n\ 121 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
135 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display)
143 click_allowed =
false;
149 usage(argv[0], NULL);
153 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_click_allowed =
true;
173 bool opt_display =
true;
176 if (getOptions(argc, argv, opt_click_allowed, opt_display) ==
false) {
182 #if defined VISP_HAVE_X11 185 #elif defined VISP_HAVE_GTK 188 #elif defined VISP_HAVE_GDI 191 #elif defined VISP_HAVE_OPENCV 202 displayInt.
init(Iint, 0, 0,
"Internal view");
203 displayExt.
init(Iext, 330, 000,
"External view");
207 double px = 500, py = 500;
208 double u0 = 150, v0 = 160;
215 std::cout << std::endl;
216 std::cout <<
"----------------------------------------------" << std::endl;
217 std::cout <<
" Test program for vpServo " << std::endl;
218 std::cout <<
" Eye-in-hand task control, articular velocity are computed" << std::endl;
219 std::cout <<
" Simulation " << std::endl;
220 std::cout <<
" task : servo 4 points " << std::endl;
221 std::cout <<
"----------------------------------------------" << std::endl;
222 std::cout << std::endl;
241 for (
unsigned i = 0; i < 4; i++)
242 externalview.
insert(point[i]);
246 for (
unsigned i = 0; i < 4; i++)
251 for (
unsigned i = 0; i < 4; i++)
279 for (
unsigned i = 0; i < 4; i++)
288 unsigned int iter = 0;
290 while (iter++ < 200) {
291 std::cout <<
"---------------------------------------------" << iter << std::endl;
305 for (
unsigned i = 0; i < 4; i++) {
332 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
338 std::cout <<
"Final robot position with respect to the object frame:\n";
341 if (opt_display && opt_click_allowed) {
348 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
352 #elif !(defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)) 355 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
361 std::cout <<
"You do not have X11, or GTK, or GDI (Graphical Device Interface) functionalities to display images..." << std::endl;
362 std::cout <<
"Tip if you are on a unix-like system:" << std::endl;
363 std::cout <<
"- Install X11, configure again ViSP using cmake and build again this example" << std::endl;
364 std::cout <<
"Tip if you are on a windows-like system:" << std::endl;
365 std::cout <<
"- Install GDI, configure again ViSP using cmake and build again this example" << std::endl;
Implementation of a matrix and operations on matrices.
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 setWorldCoordinates(double oX, double oY, double oZ)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
void buildFrom(double x, double y, double Z)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines the simplest robot: a free flying camera.
void addFeature(vpBasicFeature &s, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Display for windows using GDI (available on any windows 32 platform).
void set_eJe(const vpMatrix &eJe_)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
error that can be emited by ViSP classes.
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="")
void track(const vpHomogeneousMatrix &cMo)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
static const vpColor green
static void flush(const vpImage< unsigned char > &I)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
vpColVector getError() const
vpColVector computeControlLaw()
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
void insert(vpForwardProjection &fp)
vpHomogeneousMatrix getPosition() const
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
static double rad(double deg)
Implementation of column vector and the associated operations.
void set_cVe(const vpVelocityTwistMatrix &cVe_)
vpHomogeneousMatrix inverse() const
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
void get_eJe(vpMatrix &eJe)
interface with the image for feature display
static const vpColor white
void setServo(const vpServoType &servo_type)
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)