55 #include <visp3/core/vpCameraParameters.h>
56 #include <visp3/core/vpCylinder.h>
57 #include <visp3/core/vpHomogeneousMatrix.h>
58 #include <visp3/core/vpImage.h>
59 #include <visp3/core/vpMath.h>
60 #include <visp3/gui/vpDisplayD3D.h>
61 #include <visp3/gui/vpDisplayGDI.h>
62 #include <visp3/gui/vpDisplayGTK.h>
63 #include <visp3/gui/vpDisplayOpenCV.h>
64 #include <visp3/gui/vpDisplayX.h>
65 #include <visp3/gui/vpProjectionDisplay.h>
66 #include <visp3/io/vpParseArgv.h>
67 #include <visp3/robot/vpSimulatorCamera.h>
68 #include <visp3/visual_features/vpFeatureBuilder.h>
69 #include <visp3/visual_features/vpFeatureLine.h>
70 #include <visp3/vs/vpServo.h>
71 #include <visp3/vs/vpServoDisplay.h>
74 #define GETOPTARGS "cdho"
76 void usage(
const char *name,
const char *badparam);
77 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display);
87 void usage(
const char *name,
const char *badparam)
90 Simulation of a 2D visual servoing on a cylinder:\n\
91 - eye-in-hand control law,\n\
92 - velocity computed in the camera frame,\n\
93 - display the camera view.\n\
96 %s [-c] [-d] [-o] [-h]\n",
103 Disable the mouse click. Useful to automate the \n\
104 execution of this program without human intervention.\n\
107 Turn off the display.\n\
110 Disable new projection operator usage for secondary task.\n\
116 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
131 bool getOptions(
int argc,
const char **argv,
bool &click_allowed,
bool &display,
bool &new_proj_operator)
139 click_allowed =
false;
145 new_proj_operator =
false;
148 usage(argv[0],
nullptr);
152 usage(argv[0], optarg_);
157 if ((c == 1) || (c == -1)) {
159 usage(argv[0],
nullptr);
160 std::cerr <<
"ERROR: " << std::endl;
161 std::cerr <<
" Bad argument " << optarg_ << std::endl << std::endl;
168 int main(
int argc,
const char **argv)
170 #if (defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV))
172 bool opt_display =
true;
173 bool opt_click_allowed =
true;
174 bool opt_new_proj_operator =
true;
177 if (getOptions(argc, argv, opt_click_allowed, opt_display, opt_new_proj_operator) ==
false) {
185 #ifdef VISP_HAVE_DISPLAY
186 #if defined(VISP_HAVE_X11)
189 #elif defined(VISP_HAVE_GTK)
192 #elif defined(VISP_HAVE_GDI)
195 #elif defined(HAVE_OPENCV_HIGHGUI)
198 #elif defined(VISP_HAVE_D3D9)
205 #ifdef VISP_HAVE_DISPLAY
208 displayInt.
init(Iint, 100, 100,
"Internal view");
209 displayExt.
init(Iext, 130 +
static_cast<int>(Iint.getWidth()), 100,
"External view");
222 #ifdef VISP_HAVE_DISPLAY
241 robot.getPosition(wMc);
252 #ifdef VISP_HAVE_DISPLAY
253 externalview.
insert(cylinder);
256 cylinder.track(cMod);
262 for (
unsigned int i = 0; i < 2; i++)
273 for (
unsigned int i = 0; i < 2; i++) {
300 #ifdef VISP_HAVE_DISPLAY
309 if (opt_display && opt_click_allowed) {
321 unsigned int iter = 0;
323 bool start_secondary_task =
false;
326 std::cout <<
"---------------------------------------------" << iter++ << std::endl;
329 robot.getPosition(wMc);
341 for (
unsigned int i = 0; i < 2; i++) {
350 #ifdef VISP_HAVE_DISPLAY
360 start_secondary_task =
true;
363 if (start_secondary_task) {
377 static unsigned int iter_sec = 0;
379 double vitesse = 0.5;
380 unsigned int tempo = 800;
382 if (iter_sec > tempo) {
386 if (iter_sec % tempo < 200) {
388 e1[0] = fabs(vitesse);
390 rapport = vitesse / proj_e1[0];
395 if (iter_sec % tempo < 400 && iter_sec % tempo >= 200) {
397 e2[1] = fabs(vitesse);
399 rapport = vitesse / proj_e2[1];
404 if (iter_sec % tempo < 600 && iter_sec % tempo >= 400) {
406 e1[0] = -fabs(vitesse);
408 rapport = -vitesse / proj_e1[0];
413 if (iter_sec % tempo < 800 && iter_sec % tempo >= 600) {
415 e2[1] = -fabs(vitesse);
417 rapport = -vitesse / proj_e2[1];
422 if (opt_display && opt_click_allowed) {
423 std::stringstream ss;
424 ss << std::string(
"New projection operator: ") +
425 (opt_new_proj_operator ? std::string(
"yes (use option -o to use old one)") : std::string(
"no"));
432 if (opt_display && opt_click_allowed) {
440 std::cout <<
"|| s - s* || = " << (task.
getError()).sumSquare() << std::endl;
454 if (opt_display && opt_click_allowed) {
466 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
472 std::cout <<
"Cannot run this example: install Lapack, Eigen3 or OpenCV" << std::endl;
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor white
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
Display for windows using Direct3D 3rd party. Thus to enable this class Direct3D should be installed....
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
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...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
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 emitted by ViSP classes.
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
Class that defines a 2D line visual feature which is composed by two parameters that are and ,...
void print(unsigned int select=FEATURE_ALL) const vp_override
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
static double rad(double deg)
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
interface with the image for feature display
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 insert(vpForwardProjection &fp)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
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)
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
void print(const vpServo::vpServoPrintType display_level=ALL, std::ostream &os=std::cout)
vpColVector secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator=false)
void setServo(const vpServoType &servo_type)
vpColVector getError() const
vpColVector computeControlLaw()
Class that defines the simplest robot: a free flying camera.
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.